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

Merge branch 'borglab:develop' into develop

release/4.3a0
Catalina 2025-01-22 16:50:42 +08:00 committed by GitHub
parent 140984d2d9 d5f304ef50
commit e7c532ed31
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
51 changed files with 3064 additions and 340 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.github/scripts/unix.sh vendored
View File

@ -45,13 +45,15 @@ function configure()
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
-DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-OFF} \
-DGTSAM_ALLOW_DEPRECATED_SINCE_V43=${GTSAM_ALLOW_DEPRECATED_SINCE_V43:-OFF} \
-DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
-DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \
-DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \
-DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
-DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
-DGTSAM_USE_BOOST_FEATURES=${GTSAM_USE_BOOST_FEATURES:-ON} \
-DGTSAM_ENABLE_BOOST_SERIALIZATION=${GTSAM_ENABLE_BOOST_SERIALIZATION:-ON} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_SINGLE_TEST_EXE=OFF
}
@ -69,7 +71,6 @@ function finish ()
# compile the code with the intent of populating the cache
function build ()
{
export GTSAM_BUILD_EXAMPLES_ALWAYS=ON
export GTSAM_BUILD_TESTS=OFF
configure
@ -90,7 +91,6 @@ function build ()
# run the tests
function test ()
{
export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF
export GTSAM_BUILD_TESTS=ON
configure

29
.github/workflows/build-linux.yml vendored
View File

@ -2,8 +2,8 @@ name: Linux CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
@ -25,11 +25,12 @@ jobs:
# Github Actions requires a single row to be added to the build matrix.
# See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
name: [
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
ubuntu-22.04-gcc-12,
ubuntu-22.04-clang-14,
]
# "Bracket" the versions from GCC [9-14] and Clang [9-16]
ubuntu-20.04-gcc-9,
ubuntu-20.04-clang-9,
ubuntu-24.04-gcc-14,
ubuntu-24.04-clang-16,
]
build_type: [Debug, Release]
build_unstable: [ON]
@ -44,16 +45,16 @@ jobs:
compiler: clang
version: "9"
- name: ubuntu-22.04-gcc-12
os: ubuntu-22.04
- name: ubuntu-24.04-gcc-14
os: ubuntu-24.04
compiler: gcc
version: "11"
- name: ubuntu-22.04-clang-14
os: ubuntu-22.04
compiler: clang
version: "14"
- name: ubuntu-24.04-clang-16
os: ubuntu-24.04
compiler: clang
version: "16"
steps:
- name: Checkout
uses: actions/checkout@v4

34
.github/workflows/build-special.yml vendored
View File

@ -2,8 +2,8 @@ name: Special Cases CI
on: [pull_request]
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
# Every time you make a push to your PR, it cancel immediately the previous checks,
# and start a new one. The other runner will be available more quickly to your PR.
concurrency:
group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }}
cancel-in-progress: true
@ -34,6 +34,7 @@ jobs:
ubuntu-clang-system-libs,
ubuntu-no-boost,
ubuntu-no-unstable,
ubuntu-build-examples,
]
build_type: [Debug, Release]
@ -81,6 +82,12 @@ jobs:
version: "14"
flag: no_unstable
- name: ubuntu-build-examples
os: ubuntu-22.04
compiler: clang
version: "14"
flag: build_examples
steps:
- name: Checkout
uses: actions/checkout@v4
@ -115,19 +122,24 @@ jobs:
echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
fi
- name: Install Boost
if: runner.os == 'Linux'
run: |
sudo apt-get -y install libboost-all-dev
- name: Install (macOS)
if: runner.os == 'macOS'
run: |
brew install cmake ninja boost
brew install cmake ninja
sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app
echo "CC=clang" >> $GITHUB_ENV
echo "CXX=clang++" >> $GITHUB_ENV
- name: Install Boost
run: |
if [ ${{matrix.flag}} != 'no_boost' ]; then
if [ ${{runner.os}} == 'Linux' ]; then
sudo apt-get -y install libboost-all-dev
elif [ ${{runner.os}} == 'macOS' ]; then
brew install boost
fi
fi
- name: Set Allow Deprecated Flag
if: matrix.flag == 'deprecated'
run: |
@ -153,6 +165,11 @@ jobs:
echo "GTSAM_ROT3_EXPMAP=OFF" >> $GITHUB_ENV
echo "GTSAM Uses Cayley map for Rot3"
- name: Build Examples
if: matrix.flag == 'build_examples'
run: |
echo "GTSAM_BUILD_EXAMPLES_ALWAYS=ON" >> $GITHUB_ENV
- name: Use system versions of 3rd party libraries
if: matrix.flag == 'system'
run: |
@ -181,7 +198,6 @@ jobs:
with:
swap-size-gb: 12
- name: Build & Test
run: |
bash .github/scripts/unix.sh -t

6
Using-GTSAM-EXPORT.md
View File

@ -3,9 +3,9 @@
To create a DLL in windows, the `GTSAM_EXPORT` keyword has been created and needs to be applied to different methods and classes in the code to expose this code outside of the DLL. However, there are several intricacies that make this more difficult than it sounds. In general, if you follow the following three rules, GTSAM_EXPORT should work properly. The rest of the document also describes (1) the common error message encountered when you are not following these rules and (2) the reasoning behind these usage rules.
## Usage Rules
1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is declared in a .cpp file, not just a .h file.
1. Put `GTSAM_EXPORT` in front of any function that you want exported in the DLL _if and only if_ that function is defined in a .cpp file, not just a .h file.
2. Use `GTSAM_EXPORT` in a class definition (i.e. `class GSTAM_EXPORT MyClass {...}`) only if:
* At least one of the functions inside that class is declared in a .cpp file and not just the .h file.
* At least one of the functions inside that class is defined in a .cpp file and not just the .h file.
* You can `GTSAM_EXPORT` any class it inherits from as well. (Note that this implictly requires the class does not derive from a "header-only" class. Note that Eigen is a "header-only" library, so if your class derives from Eigen, _do not_ use `GTSAM_EXPORT` in the class definition!)
3. If you have defined a class using `GTSAM_EXPORT`, do not use `GTSAM_EXPORT` in any of its individual function declarations. (Note that you _can_ put `GTSAM_EXPORT` in the definition of individual functions within a class as long as you don't put `GTSAM_EXPORT` in the class definition.)
4. For template specializations, you need to add `GTSAM_EXPORT` to each individual specialization.
@ -28,7 +28,7 @@ But first, we need to understand exactly what `GTSAM_EXPORT` is. `GTSAM_EXPORT`
Rule #1 doesn't seem very bad, until you combine it with rule #2
***Compiler Rule #2*** Anything declared in a header file is not included in a DLL.
***Compiler Rule #2*** Anything defined in a header file is not included in a DLL.
When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class.

200
examples/Data/issue1452.txt Normal file
View File

@ -0,0 +1,200 @@
0 1645005900.600844860077 0 6.217194869853 0.125917563436 0.783594893995 -0.033789843270 0.047134300047 0.266189953411 0.000021415732 0.000001074791 -0.000000147245 -0.000000847030 0.000019167883 0.000016718464 0.000001074791 0.000001605304 -0.000000003066 -0.000001377764 0.000000939474 0.000008913959 -0.000000147245 -0.000000003066 0.000001484791 -0.000000768622 -0.000007854923 -0.000000092444 -0.000000847030 -0.000001377764 -0.000000768622 0.000104453167 0.000003272897 -0.000007611301 0.000019167883 0.000000939474 -0.000007854923 0.000003272897 0.000160225289 0.000014846515 0.000016718464 0.000008913959 -0.000000092444 -0.000007611301 0.000014846515 0.000157951799
1 1645005900.701431751251 0 1 0.005593021030 0.001526481838 -0.001566024571 -0.000225927310 0.000128593690 -0.003791450430 0.000000179286 -0.000000039948 0.000000006043 -0.000000004575 -0.000000022632 0.000000091709 -0.000000039948 0.000000018979 -0.000000001546 0.000000001926 0.000000006078 -0.000000017221 0.000000006043 -0.000000001546 0.000000015351 -0.000000013913 0.000000001925 0.000000001075 -0.000000004575 0.000000001926 -0.000000013913 0.000000561628 -0.000000028062 0.000000012214 -0.000000022632 0.000000006078 0.000000001925 -0.000000028062 0.000000490365 -0.000000005530 0.000000091709 -0.000000017221 0.000000001075 0.000000012214 -0.000000005530 0.000000550990
1 1645005900.799895286560 1 2 0.005283034558 0.001722784230 -0.000367567191 0.000386621238 0.000182655752 -0.003429626268 0.000000162458 -0.000000036144 0.000000005112 -0.000000002457 -0.000000020766 0.000000082373 -0.000000036144 0.000000017835 -0.000000001282 0.000000001448 0.000000005345 -0.000000014851 0.000000005112 -0.000000001282 0.000000014551 -0.000000013437 0.000000001790 0.000000000998 -0.000000002457 0.000000001448 -0.000000013437 0.000000523714 -0.000000027843 0.000000011143 -0.000000020766 0.000000005345 0.000000001790 -0.000000027843 0.000000456118 -0.000000005134 0.000000082373 -0.000000014851 0.000000000998 0.000000011143 -0.000000005134 0.000000510306
1 1645005900.902594566345 2 3 0.003030780414 0.000830079823 0.000494312020 -0.000020397824 0.000094347098 -0.002984943110 0.000000145900 -0.000000032354 0.000000004126 -0.000000001486 -0.000000018203 0.000000070890 -0.000000032354 0.000000016529 -0.000000001105 0.000000001395 0.000000004540 -0.000000012806 0.000000004126 -0.000000001105 0.000000014132 -0.000000012278 0.000000001993 0.000000000686 -0.000000001486 0.000000001395 -0.000000012278 0.000000469953 -0.000000025678 0.000000008948 -0.000000018203 0.000000004540 0.000000001993 -0.000000025678 0.000000410806 -0.000000003776 0.000000070890 -0.000000012806 0.000000000686 0.000000008948 -0.000000003776 0.000000445310
1 1645005901.003520727158 3 4 -0.000206245469 -0.001102739599 0.000139509744 -0.000044223949 0.000133857483 -0.002842172502 0.000000130290 -0.000000028944 0.000000003338 -0.000000000544 -0.000000016340 0.000000062247 -0.000000028944 0.000000015436 -0.000000000943 0.000000001260 0.000000004005 -0.000000011220 0.000000003338 -0.000000000943 0.000000014049 -0.000000012189 0.000000001599 0.000000000394 -0.000000000544 0.000000001260 -0.000000012189 0.000000464881 -0.000000026439 0.000000008623 -0.000000016340 0.000000004005 0.000000001599 -0.000000026439 0.000000404384 -0.000000003057 0.000000062247 -0.000000011220 0.000000000394 0.000000008623 -0.000000003057 0.000000423376
1 1645005901.101174592972 4 5 -0.001500063017 -0.002890410973 -0.000122347193 0.000014693626 0.000245148283 -0.002217688406 0.000000119368 -0.000000026625 0.000000002896 0.000000000371 -0.000000014770 0.000000055853 -0.000000026625 0.000000014763 -0.000000000823 0.000000000938 0.000000003581 -0.000000009513 0.000000002896 -0.000000000823 0.000000013680 -0.000000011988 0.000000001262 0.000000000298 0.000000000371 0.000000000938 -0.000000011988 0.000000464346 -0.000000027637 0.000000007687 -0.000000014770 0.000000003581 0.000000001262 -0.000000027637 0.000000402444 -0.000000002216 0.000000055853 -0.000000009513 0.000000000298 0.000000007687 -0.000000002216 0.000000418928
1 1645005901.201069116592 5 6 0.001046374496 -0.004032584187 -0.000846599681 0.000321181584 -0.000094749466 -0.002057391696 0.000000110823 -0.000000024485 0.000000002417 0.000000001106 -0.000000012269 0.000000051317 -0.000000024485 0.000000014167 -0.000000000692 0.000000000635 0.000000003038 -0.000000008394 0.000000002417 -0.000000000692 0.000000013921 -0.000000011892 0.000000000364 0.000000000007 0.000000001106 0.000000000635 -0.000000011892 0.000000448440 -0.000000027796 0.000000007091 -0.000000012269 0.000000003038 0.000000000364 -0.000000027796 0.000000388841 -0.000000000851 0.000000051317 -0.000000008394 0.000000000007 0.000000007091 -0.000000000851 0.000000405686
1 1645005901.302946805954 6 7 0.000793901956 -0.003389991954 -0.001184675507 -0.000316637882 0.000125753514 -0.001152057016 0.000000104712 -0.000000022968 0.000000001967 0.000000001772 -0.000000010574 0.000000048337 -0.000000022968 0.000000013707 -0.000000000599 0.000000000391 0.000000002734 -0.000000007830 0.000000001967 -0.000000000599 0.000000014353 -0.000000012287 -0.000000001119 -0.000000000386 0.000000001772 0.000000000391 -0.000000012287 0.000000430486 -0.000000025901 0.000000008441 -0.000000010574 0.000000002734 -0.000000001119 -0.000000025901 0.000000374002 0.000000000168 0.000000048337 -0.000000007830 -0.000000000386 0.000000008441 0.000000000168 0.000000386878
1 1645005901.403146505356 7 8 0.000422785492 -0.001577894861 -0.000916262031 0.000148651815 -0.000064627432 -0.001220538595 0.000000100167 -0.000000021927 0.000000002289 0.000000000677 -0.000000011203 0.000000045605 -0.000000021927 0.000000013460 -0.000000000705 0.000000000708 0.000000002868 -0.000000007296 0.000000002289 -0.000000000705 0.000000014003 -0.000000011457 -0.000000000961 -0.000000000148 0.000000000677 0.000000000708 -0.000000011457 0.000000410964 -0.000000025258 0.000000008570 -0.000000011203 0.000000002868 -0.000000000961 -0.000000025258 0.000000357815 -0.000000000299 0.000000045605 -0.000000007296 -0.000000000148 0.000000008570 -0.000000000299 0.000000367628
1 1645005901.502384662628 8 9 -0.002329688882 -0.000140222280 0.000009892231 -0.000239651077 0.000253304248 -0.001105202980 0.000000095969 -0.000000020860 0.000000002507 -0.000000000886 -0.000000012048 0.000000043109 -0.000000020860 0.000000013064 -0.000000000726 0.000000001166 0.000000002962 -0.000000007369 0.000000002507 -0.000000000726 0.000000013480 -0.000000010270 -0.000000000517 0.000000000184 -0.000000000886 0.000000001166 -0.000000010270 0.000000387773 -0.000000024930 0.000000007342 -0.000000012048 0.000000002962 -0.000000000517 -0.000000024930 0.000000338486 -0.000000001780 0.000000043109 -0.000000007369 0.000000000184 0.000000007342 -0.000000001780 0.000000347313
1 1645005901.599597692490 9 10 -0.004026581327 0.000969420632 0.001171132830 0.000030066830 -0.000042684471 -0.001078015407 0.000000092798 -0.000000020050 0.000000002088 -0.000000000514 -0.000000011837 0.000000040468 -0.000000020050 0.000000012604 -0.000000000661 0.000000001084 0.000000002794 -0.000000007809 0.000000002088 -0.000000000661 0.000000013337 -0.000000009999 -0.000000000185 0.000000000097 -0.000000000514 0.000000001084 -0.000000009999 0.000000364337 -0.000000023644 0.000000006385 -0.000000011837 0.000000002794 -0.000000000185 -0.000000023644 0.000000320215 -0.000000002428 0.000000040468 -0.000000007809 0.000000000097 0.000000006385 -0.000000002428 0.000000324189
1 1645005901.700814247131 10 11 -0.003099084350 0.000842028850 0.001191700627 -0.000180302292 -0.000304391953 -0.000943844689 0.000000090213 -0.000000019520 0.000000001928 -0.000000000067 -0.000000011416 0.000000037331 -0.000000019520 0.000000012352 -0.000000000665 0.000000000889 0.000000002684 -0.000000007936 0.000000001928 -0.000000000665 0.000000013403 -0.000000009790 0.000000000272 0.000000000027 -0.000000000067 0.000000000889 -0.000000009790 0.000000343339 -0.000000021912 0.000000005787 -0.000000011416 0.000000002684 0.000000000272 -0.000000021912 0.000000304258 -0.000000002229 0.000000037331 -0.000000007936 0.000000000027 0.000000005787 -0.000000002229 0.000000301522
1 1645005901.799977779388 11 12 -0.001119124717 0.000911769018 0.000896053010 0.000109146445 0.000004749543 -0.001769999687 0.000000088504 -0.000000019214 0.000000002010 -0.000000000364 -0.000000011599 0.000000035324 -0.000000019214 0.000000012281 -0.000000000663 0.000000000986 0.000000002726 -0.000000007878 0.000000002010 -0.000000000663 0.000000013725 -0.000000009806 0.000000000042 0.000000000058 -0.000000000364 0.000000000986 -0.000000009806 0.000000328413 -0.000000018998 0.000000005385 -0.000000011599 0.000000002726 0.000000000042 -0.000000018998 0.000000293251 -0.000000002089 0.000000035324 -0.000000007878 0.000000000058 0.000000005385 -0.000000002089 0.000000286449
1 1645005901.900312423706 12 13 -0.000402732963 0.000242598646 -0.000072269012 0.000055999966 0.000123729242 -0.002073753983 0.000000087170 -0.000000018926 0.000000002027 -0.000000000440 -0.000000011562 0.000000034056 -0.000000018926 0.000000012165 -0.000000000686 0.000000001149 0.000000002703 -0.000000007746 0.000000002027 -0.000000000686 0.000000013717 -0.000000009355 -0.000000000107 0.000000000029 -0.000000000440 0.000000001149 -0.000000009355 0.000000315929 -0.000000016862 0.000000005085 -0.000000011562 0.000000002703 -0.000000000107 -0.000000016862 0.000000284663 -0.000000002049 0.000000034056 -0.000000007746 0.000000000029 0.000000005085 -0.000000002049 0.000000275236
1 1645005901.999710559845 13 14 -0.000353031834 0.000520456797 -0.000885039005 0.000175471614 0.000226437074 -0.001781941944 0.000000085576 -0.000000018640 0.000000002007 -0.000000000556 -0.000000011265 0.000000032750 -0.000000018640 0.000000011971 -0.000000000692 0.000000001200 0.000000002700 -0.000000007525 0.000000002007 -0.000000000692 0.000000013341 -0.000000008639 0.000000000083 -0.000000000060 -0.000000000556 0.000000001200 -0.000000008639 0.000000304286 -0.000000016247 0.000000004967 -0.000000011265 0.000000002700 0.000000000083 -0.000000016247 0.000000275471 -0.000000002124 0.000000032750 -0.000000007525 -0.000000000060 0.000000004967 -0.000000002124 0.000000264734
1 1645005902.100522041321 14 15 -0.000755912182 0.000162757993 -0.000940218374 0.000142873662 0.000156455499 -0.000999359978 0.000000084547 -0.000000018524 0.000000002007 -0.000000001342 -0.000000011413 0.000000031398 -0.000000018524 0.000000011963 -0.000000000637 0.000000001412 0.000000002758 -0.000000007386 0.000000002007 -0.000000000637 0.000000012995 -0.000000008153 0.000000000029 -0.000000000020 -0.000000001342 0.000000001412 -0.000000008153 0.000000294679 -0.000000015478 0.000000004679 -0.000000011413 0.000000002758 0.000000000029 -0.000000015478 0.000000268528 -0.000000002296 0.000000031398 -0.000000007386 -0.000000000020 0.000000004679 -0.000000002296 0.000000256957
1 1645005902.203018426895 15 16 -0.000439249231 -0.000178254566 -0.000437587780 -0.000084581729 0.000049443904 -0.000079088693 0.000000083829 -0.000000018377 0.000000001938 -0.000000001650 -0.000000011605 0.000000030281 -0.000000018377 0.000000011902 -0.000000000597 0.000000001442 0.000000002713 -0.000000007253 0.000000001938 -0.000000000597 0.000000013076 -0.000000008171 0.000000000037 0.000000000101 -0.000000001650 0.000000001442 -0.000000008171 0.000000287437 -0.000000014704 0.000000003963 -0.000000011605 0.000000002713 0.000000000037 -0.000000014704 0.000000263922 -0.000000002586 0.000000030281 -0.000000007253 0.000000000101 0.000000003963 -0.000000002586 0.000000250716
1 1645005902.301831007004 16 17 0.000298386384 -0.000560355605 -0.000119106950 0.000156455585 -0.000010964095 -0.000026764071 0.000000083200 -0.000000018095 0.000000001879 -0.000000001309 -0.000000011836 0.000000029821 -0.000000018095 0.000000011725 -0.000000000623 0.000000001381 0.000000002667 -0.000000006986 0.000000001879 -0.000000000623 0.000000013155 -0.000000008373 0.000000000404 0.000000000134 -0.000000001309 0.000000001381 -0.000000008373 0.000000283842 -0.000000014501 0.000000003889 -0.000000011836 0.000000002667 0.000000000404 -0.000000014501 0.000000261244 -0.000000002864 0.000000029821 -0.000000006986 0.000000000134 0.000000003889 -0.000000002864 0.000000247086
1 1645005902.400156021118 17 18 0.000179886078 -0.000459777112 -0.000358763747 -0.000153694839 0.000090517348 0.000016912731 0.000000081953 -0.000000017650 0.000000001880 -0.000000001460 -0.000000011911 0.000000029712 -0.000000017650 0.000000011542 -0.000000000621 0.000000001450 0.000000002686 -0.000000006772 0.000000001880 -0.000000000621 0.000000012815 -0.000000008204 0.000000000400 0.000000000134 -0.000000001460 0.000000001450 -0.000000008204 0.000000279237 -0.000000013903 0.000000003850 -0.000000011911 0.000000002686 0.000000000400 -0.000000013903 0.000000257257 -0.000000002996 0.000000029712 -0.000000006772 0.000000000134 0.000000003850 -0.000000002996 0.000000243174
1 1645005902.500535726547 18 19 -0.000677739243 -0.000661804764 -0.000786983512 0.000083215128 -0.000114303323 0.000518990892 0.000000081125 -0.000000017513 0.000000001940 -0.000000001754 -0.000000011942 0.000000029299 -0.000000017513 0.000000011479 -0.000000000613 0.000000001489 0.000000002719 -0.000000006813 0.000000001940 -0.000000000613 0.000000012630 -0.000000007967 0.000000000014 0.000000000174 -0.000000001754 0.000000001489 -0.000000007967 0.000000273527 -0.000000013282 0.000000003515 -0.000000011942 0.000000002719 0.000000000014 -0.000000013282 0.000000252050 -0.000000003271 0.000000029299 -0.000000006813 0.000000000174 0.000000003515 -0.000000003271 0.000000239150
1 1645005902.600610494614 19 20 -0.001398677644 -0.000281431524 -0.000006292086 -0.000149844144 0.000155174368 0.000310385345 0.000000081164 -0.000000017520 0.000000001984 -0.000000001776 -0.000000012440 0.000000028693 -0.000000017520 0.000000011567 -0.000000000605 0.000000001477 0.000000002823 -0.000000006788 0.000000001984 -0.000000000605 0.000000012243 -0.000000007646 -0.000000000218 0.000000000207 -0.000000001776 0.000000001477 -0.000000007646 0.000000270203 -0.000000013042 0.000000003281 -0.000000012440 0.000000002823 -0.000000000218 -0.000000013042 0.000000249116 -0.000000003680 0.000000028693 -0.000000006788 0.000000000207 0.000000003281 -0.000000003680 0.000000237404
1 1645005902.700582027435 20 21 -0.001420482214 -0.000263135886 -0.000053375571 -0.000116154210 -0.000141423619 0.000057516294 0.000000080767 -0.000000017432 0.000000001927 -0.000000001700 -0.000000012707 0.000000028372 -0.000000017432 0.000000011550 -0.000000000579 0.000000001468 0.000000002900 -0.000000006723 0.000000001927 -0.000000000579 0.000000011880 -0.000000007357 -0.000000000236 0.000000000169 -0.000000001700 0.000000001468 -0.000000007357 0.000000268424 -0.000000012714 0.000000003151 -0.000000012707 0.000000002900 -0.000000000236 -0.000000012714 0.000000247501 -0.000000003752 0.000000028372 -0.000000006723 0.000000000169 0.000000003151 -0.000000003752 0.000000236011
1 1645005902.802828073502 21 22 -0.001522845641 -0.000074472171 0.000173559055 -0.000051052057 -0.000127920939 -0.000114638340 0.000000080331 -0.000000017568 0.000000001879 -0.000000001733 -0.000000012773 0.000000027688 -0.000000017568 0.000000011534 -0.000000000541 0.000000001430 0.000000002960 -0.000000006720 0.000000001879 -0.000000000541 0.000000011888 -0.000000007198 -0.000000000298 0.000000000147 -0.000000001733 0.000000001430 -0.000000007198 0.000000266369 -0.000000012230 0.000000003032 -0.000000012773 0.000000002960 -0.000000000298 -0.000000012230 0.000000246106 -0.000000003739 0.000000027688 -0.000000006720 0.000000000147 0.000000003032 -0.000000003739 0.000000233014
1 1645005902.900591373444 22 23 -0.000662864106 -0.000366807912 -0.000191751859 0.000099874707 0.000044070199 -0.000205737831 0.000000079920 -0.000000017596 0.000000001894 -0.000000001895 -0.000000012932 0.000000026867 -0.000000017596 0.000000011473 -0.000000000544 0.000000001444 0.000000002996 -0.000000006939 0.000000001894 -0.000000000544 0.000000011681 -0.000000007215 -0.000000000519 0.000000000192 -0.000000001895 0.000000001444 -0.000000007215 0.000000264971 -0.000000011324 0.000000002987 -0.000000012932 0.000000002996 -0.000000000519 -0.000000011324 0.000000246113 -0.000000003809 0.000000026867 -0.000000006939 0.000000000192 0.000000002987 -0.000000003809 0.000000230070
1 1645005902.999960184097 23 24 0.000586459972 -0.000383883637 -0.000657440721 0.000025502157 0.000173096398 -0.000657422729 0.000000078961 -0.000000017438 0.000000001897 -0.000000001864 -0.000000012901 0.000000026255 -0.000000017438 0.000000011356 -0.000000000570 0.000000001429 0.000000002941 -0.000000007077 0.000000001897 -0.000000000570 0.000000011277 -0.000000007081 -0.000000000639 0.000000000279 -0.000000001864 0.000000001429 -0.000000007081 0.000000261573 -0.000000010356 0.000000003006 -0.000000012901 0.000000002941 -0.000000000639 -0.000000010356 0.000000244298 -0.000000003671 0.000000026255 -0.000000007077 0.000000000279 0.000000003006 -0.000000003671 0.000000226695
1 1645005903.099860429764 24 25 0.001039569017 -0.000664462296 -0.000116140678 -0.000029188969 0.000121233801 -0.000658346529 0.000000078315 -0.000000017330 0.000000001958 -0.000000001934 -0.000000012838 0.000000025439 -0.000000017330 0.000000011376 -0.000000000569 0.000000001357 0.000000002951 -0.000000007067 0.000000001958 -0.000000000569 0.000000011058 -0.000000006788 -0.000000000667 0.000000000313 -0.000000001934 0.000000001357 -0.000000006788 0.000000257200 -0.000000009782 0.000000002843 -0.000000012838 0.000000002951 -0.000000000667 -0.000000009782 0.000000240971 -0.000000003535 0.000000025439 -0.000000007067 0.000000000313 0.000000002843 -0.000000003535 0.000000223420
1 1645005903.201798200607 25 26 0.000799401277 -0.000715390089 -0.000135250388 -0.000005401473 0.000028317278 -0.000705012899 0.000000078273 -0.000000017284 0.000000001945 -0.000000002002 -0.000000012892 0.000000025018 -0.000000017284 0.000000011434 -0.000000000564 0.000000001376 0.000000002920 -0.000000007058 0.000000001945 -0.000000000564 0.000000011188 -0.000000006747 -0.000000000284 0.000000000359 -0.000000002002 0.000000001376 -0.000000006747 0.000000255919 -0.000000009831 0.000000002438 -0.000000012892 0.000000002920 -0.000000000284 -0.000000009831 0.000000239627 -0.000000003371 0.000000025018 -0.000000007058 0.000000000359 0.000000002438 -0.000000003371 0.000000222579
1 1645005903.299434661865 26 27 -0.000113261770 0.000023693570 -0.000214375044 -0.000117559997 0.000124720787 -0.000596143614 0.000000078325 -0.000000017305 0.000000001849 -0.000000001888 -0.000000012882 0.000000024989 -0.000000017305 0.000000011476 -0.000000000557 0.000000001385 0.000000002888 -0.000000006966 0.000000001849 -0.000000000557 0.000000011561 -0.000000006807 0.000000000150 0.000000000342 -0.000000001888 0.000000001385 -0.000000006807 0.000000256583 -0.000000010009 0.000000002389 -0.000000012882 0.000000002888 0.000000000150 -0.000000010009 0.000000240356 -0.000000003042 0.000000024989 -0.000000006966 0.000000000342 0.000000002389 -0.000000003042 0.000000222999
1 1645005903.399843454361 27 28 -0.000958900034 0.000637600325 -0.000214249917 -0.000223045645 -0.000071809246 0.000029335082 0.000000078344 -0.000000017286 0.000000001846 -0.000000001681 -0.000000012791 0.000000024604 -0.000000017286 0.000000011476 -0.000000000598 0.000000001367 0.000000002926 -0.000000006949 0.000000001846 -0.000000000598 0.000000012087 -0.000000006902 0.000000000466 0.000000000207 -0.000000001681 0.000000001367 -0.000000006902 0.000000256077 -0.000000009887 0.000000002620 -0.000000012791 0.000000002926 0.000000000466 -0.000000009887 0.000000240846 -0.000000002625 0.000000024604 -0.000000006949 0.000000000207 0.000000002620 -0.000000002625 0.000000222033
1 1645005903.503298759460 28 29 -0.000594952889 0.000937675663 0.000350959010 -0.000134047994 0.000112568416 0.000532529091 0.000000077870 -0.000000017120 0.000000001906 -0.000000001471 -0.000000012767 0.000000024191 -0.000000017120 0.000000011459 -0.000000000663 0.000000001370 0.000000002979 -0.000000007036 0.000000001906 -0.000000000663 0.000000012464 -0.000000006974 0.000000000554 0.000000000084 -0.000000001471 0.000000001370 -0.000000006974 0.000000254804 -0.000000009282 0.000000002485 -0.000000012767 0.000000002979 0.000000000554 -0.000000009282 0.000000241012 -0.000000002108 0.000000024191 -0.000000007036 0.000000000084 0.000000002485 -0.000000002108 0.000000220716
1 1645005903.600738525391 29 30 0.000794414323 0.000340891713 0.000722517724 0.000049267232 -0.000083827634 0.000511848534 0.000000077849 -0.000000017062 0.000000001944 -0.000000001282 -0.000000012931 0.000000024026 -0.000000017062 0.000000011499 -0.000000000672 0.000000001311 0.000000003010 -0.000000007173 0.000000001944 -0.000000000672 0.000000012279 -0.000000007093 0.000000000279 0.000000000086 -0.000000001282 0.000000001311 -0.000000007093 0.000000255543 -0.000000008713 0.000000002207 -0.000000012931 0.000000003010 0.000000000279 -0.000000008713 0.000000242511 -0.000000001802 0.000000024026 -0.000000007173 0.000000000086 0.000000002207 -0.000000001802 0.000000221948
1 1645005903.702013492584 30 31 0.000724079430 -0.000003591352 0.000270122431 -0.000012632068 -0.000052326779 0.000440294104 0.000000077508 -0.000000016995 0.000000001961 -0.000000001247 -0.000000012890 0.000000023883 -0.000000016995 0.000000011411 -0.000000000627 0.000000001216 0.000000003013 -0.000000007068 0.000000001961 -0.000000000627 0.000000011934 -0.000000007149 -0.000000000010 0.000000000107 -0.000000001247 0.000000001216 -0.000000007149 0.000000254403 -0.000000008732 0.000000001995 -0.000000012890 0.000000003013 -0.000000000010 -0.000000008732 0.000000240911 -0.000000001885 0.000000023883 -0.000000007068 0.000000000107 0.000000001995 -0.000000001885 0.000000221291
1 1645005903.800774335861 31 32 0.001067662490 -0.000510502525 -0.000301534356 0.000174567922 0.000065476921 -0.000090436674 0.000000077110 -0.000000016957 0.000000001949 -0.000000001388 -0.000000012566 0.000000024020 -0.000000016957 0.000000011352 -0.000000000587 0.000000001174 0.000000002958 -0.000000006851 0.000000001949 -0.000000000587 0.000000011556 -0.000000007045 -0.000000000114 0.000000000144 -0.000000001388 0.000000001174 -0.000000007045 0.000000252509 -0.000000008765 0.000000001947 -0.000000012566 0.000000002958 -0.000000000114 -0.000000008765 0.000000238424 -0.000000002141 0.000000024020 -0.000000006851 0.000000000144 0.000000001947 -0.000000002141 0.000000219630
1 1645005903.899410247803 32 33 0.000817144389 -0.001122214966 -0.000884357956 -0.000001891347 0.000036718074 0.000252289181 0.000000077446 -0.000000017113 0.000000001881 -0.000000001342 -0.000000012421 0.000000024153 -0.000000017113 0.000000011435 -0.000000000562 0.000000001165 0.000000002871 -0.000000006813 0.000000001881 -0.000000000562 0.000000011375 -0.000000007060 -0.000000000151 0.000000000211 -0.000000001342 0.000000001165 -0.000000007060 0.000000252108 -0.000000008770 0.000000002126 -0.000000012421 0.000000002871 -0.000000000151 -0.000000008770 0.000000237324 -0.000000002342 0.000000024153 -0.000000006813 0.000000000211 0.000000002126 -0.000000002342 0.000000219157
1 1645005903.999286413193 33 34 0.000286745156 -0.000755201087 -0.000483958459 0.000073063588 -0.000069345847 0.000131138848 0.000000077516 -0.000000017113 0.000000001949 -0.000000001353 -0.000000012656 0.000000023945 -0.000000017113 0.000000011475 -0.000000000564 0.000000001201 0.000000002844 -0.000000006895 0.000000001949 -0.000000000564 0.000000011242 -0.000000006940 -0.000000000110 0.000000000316 -0.000000001353 0.000000001201 -0.000000006940 0.000000251401 -0.000000008774 0.000000002151 -0.000000012656 0.000000002844 -0.000000000110 -0.000000008774 0.000000235544 -0.000000002538 0.000000023945 -0.000000006895 0.000000000316 0.000000002151 -0.000000002538 0.000000218001
1 1645005904.101194381714 34 35 -0.000092729542 -0.000721176305 -0.000046422003 0.000080600128 -0.000098482186 0.000269102398 0.000000077508 -0.000000016949 0.000000002044 -0.000000001517 -0.000000012907 0.000000023822 -0.000000016949 0.000000011511 -0.000000000628 0.000000001343 0.000000002868 -0.000000006931 0.000000002044 -0.000000000628 0.000000010979 -0.000000006697 -0.000000000142 0.000000000368 -0.000000001517 0.000000001343 -0.000000006697 0.000000251510 -0.000000008827 0.000000002031 -0.000000012907 0.000000002868 -0.000000000142 -0.000000008827 0.000000234984 -0.000000002474 0.000000023822 -0.000000006931 0.000000000368 0.000000002031 -0.000000002474 0.000000217838
1 1645005904.202036380768 35 36 0.000159501241 -0.000619989049 -0.000182200023 -0.000130638892 -0.000030290097 0.000441918495 0.000000077457 -0.000000016871 0.000000002049 -0.000000001653 -0.000000012863 0.000000024008 -0.000000016871 0.000000011440 -0.000000000666 0.000000001365 0.000000002895 -0.000000006831 0.000000002049 -0.000000000666 0.000000010881 -0.000000006644 -0.000000000477 0.000000000349 -0.000000001653 0.000000001365 -0.000000006644 0.000000252370 -0.000000009051 0.000000002132 -0.000000012863 0.000000002895 -0.000000000477 -0.000000009051 0.000000236368 -0.000000002172 0.000000024008 -0.000000006831 0.000000000349 0.000000002132 -0.000000002172 0.000000219216
1 1645005904.301526308060 36 37 0.000327836677 -0.000353330603 -0.000334050825 -0.000048653573 -0.000017134077 0.000406296921 0.000000077743 -0.000000017107 0.000000002089 -0.000000001553 -0.000000013005 0.000000024196 -0.000000017107 0.000000011432 -0.000000000665 0.000000001162 0.000000002958 -0.000000006693 0.000000002089 -0.000000000665 0.000000010877 -0.000000006677 -0.000000000574 0.000000000388 -0.000000001553 0.000000001162 -0.000000006677 0.000000252991 -0.000000009267 0.000000002202 -0.000000013005 0.000000002958 -0.000000000574 -0.000000009267 0.000000237868 -0.000000002187 0.000000024196 -0.000000006693 0.000000000388 0.000000002202 -0.000000002187 0.000000220113
1 1645005904.403170108795 37 38 0.000737980852 -0.000351033561 -0.000295585445 -0.000025607001 -0.000042435192 0.000431463920 0.000000077856 -0.000000017205 0.000000002133 -0.000000001519 -0.000000013172 0.000000024074 -0.000000017205 0.000000011434 -0.000000000607 0.000000001084 0.000000003078 -0.000000006696 0.000000002133 -0.000000000607 0.000000011189 -0.000000006857 -0.000000000557 0.000000000291 -0.000000001519 0.000000001084 -0.000000006857 0.000000252632 -0.000000009363 0.000000002170 -0.000000013172 0.000000003078 -0.000000000557 -0.000000009363 0.000000237409 -0.000000002500 0.000000024074 -0.000000006696 0.000000000291 0.000000002170 -0.000000002500 0.000000219313
1 1645005904.501605749130 38 39 0.001088348237 -0.000613129911 0.000232566844 0.000179265756 -0.000069468820 -0.000092368701 0.000000077918 -0.000000017330 0.000000002087 -0.000000001727 -0.000000013339 0.000000023890 -0.000000017330 0.000000011400 -0.000000000554 0.000000001171 0.000000003171 -0.000000006655 0.000000002087 -0.000000000554 0.000000011497 -0.000000006951 -0.000000000628 0.000000000205 -0.000000001727 0.000000001171 -0.000000006951 0.000000252754 -0.000000009318 0.000000002086 -0.000000013339 0.000000003171 -0.000000000628 -0.000000009318 0.000000237371 -0.000000002624 0.000000023890 -0.000000006655 0.000000000205 0.000000002086 -0.000000002624 0.000000219216
1 1645005904.599536895752 39 40 0.000468476145 -0.001272947114 0.000093562933 -0.000116852710 0.000115936099 0.000225755043 0.000000077895 -0.000000017350 0.000000001892 -0.000000001532 -0.000000013201 0.000000023949 -0.000000017350 0.000000011402 -0.000000000560 0.000000001138 0.000000003123 -0.000000006687 0.000000001892 -0.000000000560 0.000000011557 -0.000000006985 -0.000000000681 0.000000000172 -0.000000001532 0.000000001138 -0.000000006985 0.000000252449 -0.000000009335 0.000000002069 -0.000000013201 0.000000003123 -0.000000000681 -0.000000009335 0.000000237483 -0.000000002381 0.000000023949 -0.000000006687 0.000000000172 0.000000002069 -0.000000002381 0.000000218909
1 1645005904.700893878937 40 41 0.000860573463 -0.001209131350 -0.000501116645 0.000157555541 0.000010319357 -0.000048830165 0.000000077189 -0.000000017111 0.000000001814 -0.000000001358 -0.000000012728 0.000000023995 -0.000000017111 0.000000011307 -0.000000000533 0.000000001084 0.000000002997 -0.000000006770 0.000000001814 -0.000000000533 0.000000011517 -0.000000007033 -0.000000000879 0.000000000155 -0.000000001358 0.000000001084 -0.000000007033 0.000000250581 -0.000000009543 0.000000002194 -0.000000012728 0.000000002997 -0.000000000879 -0.000000009543 0.000000236400 -0.000000002342 0.000000023995 -0.000000006770 0.000000000155 0.000000002194 -0.000000002342 0.000000217604
1 1645005904.802378892899 41 42 0.000632138609 -0.000790062715 -0.000690771042 0.000034390734 -0.000109221344 0.000029091074 0.000000077001 -0.000000017060 0.000000001956 -0.000000001694 -0.000000012495 0.000000024027 -0.000000017060 0.000000011341 -0.000000000521 0.000000001130 0.000000002989 -0.000000006785 0.000000001956 -0.000000000521 0.000000011397 -0.000000006946 -0.000000000911 0.000000000171 -0.000000001694 0.000000001130 -0.000000006946 0.000000250277 -0.000000009613 0.000000002318 -0.000000012495 0.000000002989 -0.000000000911 -0.000000009613 0.000000236262 -0.000000002568 0.000000024027 -0.000000006785 0.000000000171 0.000000002318 -0.000000002568 0.000000218078
1 1645005904.902300596237 42 43 0.000802579583 -0.000538433330 -0.000341299568 0.000007003048 -0.000032678783 -0.000429464507 0.000000077294 -0.000000017051 0.000000002005 -0.000000001928 -0.000000012622 0.000000024150 -0.000000017051 0.000000011388 -0.000000000568 0.000000001229 0.000000003054 -0.000000006816 0.000000002005 -0.000000000568 0.000000011301 -0.000000006823 -0.000000000888 0.000000000138 -0.000000001928 0.000000001229 -0.000000006823 0.000000251648 -0.000000009299 0.000000002429 -0.000000012622 0.000000003054 -0.000000000888 -0.000000009299 0.000000237177 -0.000000002580 0.000000024150 -0.000000006816 0.000000000138 0.000000002429 -0.000000002580 0.000000218935
1 1645005904.999253749847 43 44 0.000501016980 0.000211957757 -0.000005361234 0.000063883092 -0.000036266373 -0.000504158455 0.000000077290 -0.000000017104 0.000000001982 -0.000000001896 -0.000000012750 0.000000024173 -0.000000017104 0.000000011394 -0.000000000596 0.000000001289 0.000000003111 -0.000000006794 0.000000001982 -0.000000000596 0.000000011045 -0.000000006670 -0.000000001063 0.000000000093 -0.000000001896 0.000000001289 -0.000000006670 0.000000252269 -0.000000009368 0.000000002660 -0.000000012750 0.000000003111 -0.000000001063 -0.000000009368 0.000000236670 -0.000000002412 0.000000024173 -0.000000006794 0.000000000093 0.000000002660 -0.000000002412 0.000000218725
1 1645005905.101718425751 44 45 -0.000024589931 0.000754394551 0.000408641144 -0.000202334514 0.000016741619 -0.000672688351 0.000000076750 -0.000000016988 0.000000001970 -0.000000001786 -0.000000012675 0.000000024005 -0.000000016988 0.000000011345 -0.000000000542 0.000000001221 0.000000003052 -0.000000006750 0.000000001970 -0.000000000542 0.000000010830 -0.000000006614 -0.000000000937 0.000000000130 -0.000000001786 0.000000001221 -0.000000006614 0.000000251233 -0.000000009933 0.000000002642 -0.000000012675 0.000000003052 -0.000000000937 -0.000000009933 0.000000234262 -0.000000002344 0.000000024005 -0.000000006750 0.000000000130 0.000000002642 -0.000000002344 0.000000217464
1 1645005905.201553344727 45 46 -0.000215612364 0.000834606698 0.000149325139 -0.000059117112 -0.000021374139 -0.000795866284 0.000000076660 -0.000000016912 0.000000001943 -0.000000001779 -0.000000012647 0.000000023970 -0.000000016912 0.000000011337 -0.000000000541 0.000000001223 0.000000003001 -0.000000006808 0.000000001943 -0.000000000541 0.000000011099 -0.000000006710 -0.000000000647 0.000000000160 -0.000000001779 0.000000001223 -0.000000006710 0.000000251867 -0.000000010134 0.000000002780 -0.000000012647 0.000000003001 -0.000000000647 -0.000000010134 0.000000234786 -0.000000002355 0.000000023970 -0.000000006808 0.000000000160 0.000000002780 -0.000000002355 0.000000217751
1 1645005905.298557758331 46 47 0.000863242831 0.001318878065 -0.000252232506 -0.000118643047 0.000022995563 -0.001197291392 0.000000077249 -0.000000017059 0.000000001925 -0.000000001737 -0.000000012443 0.000000024240 -0.000000017059 0.000000011387 -0.000000000615 0.000000001270 0.000000002946 -0.000000006834 0.000000001925 -0.000000000615 0.000000011672 -0.000000006817 -0.000000000141 0.000000000151 -0.000000001737 0.000000001270 -0.000000006817 0.000000253335 -0.000000010236 0.000000003256 -0.000000012443 0.000000002946 -0.000000000141 -0.000000010236 0.000000237176 -0.000000002293 0.000000024240 -0.000000006834 0.000000000151 0.000000003256 -0.000000002293 0.000000219058
1 1645005905.399083614349 47 48 0.002259440161 0.001105945552 -0.000750095487 0.000323798878 0.000255879602 -0.002485082179 0.000000077105 -0.000000016955 0.000000001874 -0.000000001539 -0.000000012289 0.000000024392 -0.000000016955 0.000000011391 -0.000000000645 0.000000001253 0.000000002881 -0.000000006702 0.000000001874 -0.000000000645 0.000000012203 -0.000000006954 0.000000000290 0.000000000129 -0.000000001539 0.000000001253 -0.000000006954 0.000000252527 -0.000000010460 0.000000003072 -0.000000012289 0.000000002881 0.000000000290 -0.000000010460 0.000000237679 -0.000000002199 0.000000024392 -0.000000006702 0.000000000129 0.000000003072 -0.000000002199 0.000000218901
1 1645005905.499478816986 48 49 0.001203207953 0.000792305972 -0.001325455405 0.000034228402 -0.000060355459 -0.004038449411 0.000000076987 -0.000000016850 0.000000001884 -0.000000001681 -0.000000012438 0.000000024471 -0.000000016850 0.000000011498 -0.000000000626 0.000000001299 0.000000002943 -0.000000006512 0.000000001884 -0.000000000626 0.000000012768 -0.000000007178 0.000000000282 0.000000000078 -0.000000001681 0.000000001299 -0.000000007178 0.000000253439 -0.000000010315 0.000000002860 -0.000000012438 0.000000002943 0.000000000282 -0.000000010315 0.000000238861 -0.000000002267 0.000000024471 -0.000000006512 0.000000000078 0.000000002860 -0.000000002267 0.000000220135
1 1645005905.599982023239 49 50 -0.000427727796 0.001474783510 -0.000181500876 0.000232089095 0.000542383436 -0.007766826389 0.000000077535 -0.000000017038 0.000000001838 -0.000000001917 -0.000000012698 0.000000024720 -0.000000017038 0.000000011645 -0.000000000643 0.000000001385 0.000000003075 -0.000000006360 0.000000001838 -0.000000000643 0.000000012879 -0.000000007370 -0.000000000087 -0.000000000001 -0.000000001917 0.000000001385 -0.000000007370 0.000000256358 -0.000000009978 0.000000002881 -0.000000012698 0.000000003075 -0.000000000087 -0.000000009978 0.000000240935 -0.000000002358 0.000000024720 -0.000000006360 -0.000000000001 0.000000002881 -0.000000002358 0.000000222306
1 1645005905.699927568436 50 51 -0.003379224466 0.003178186222 -0.000934051059 0.000195152956 0.000761914950 -0.011833240191 0.000000077971 -0.000000017303 0.000000001797 -0.000000001904 -0.000000012847 0.000000024954 -0.000000017303 0.000000011820 -0.000000000663 0.000000001463 0.000000003093 -0.000000006376 0.000000001797 -0.000000000663 0.000000012974 -0.000000007270 0.000000000071 0.000000000011 -0.000000001904 0.000000001463 -0.000000007270 0.000000257764 -0.000000010219 0.000000002728 -0.000000012847 0.000000003093 0.000000000071 -0.000000010219 0.000000242472 -0.000000002420 0.000000024954 -0.000000006376 0.000000000011 0.000000002728 -0.000000002420 0.000000223682
1 1645005905.802299499512 51 52 -0.003954280518 0.004558444476 -0.000638200694 0.000610985838 0.000592963225 -0.016423441061 0.000000078171 -0.000000017308 0.000000001851 -0.000000001684 -0.000000012949 0.000000025066 -0.000000017308 0.000000012067 -0.000000000670 0.000000001552 0.000000003016 -0.000000006564 0.000000001851 -0.000000000670 0.000000013096 -0.000000007042 0.000000000544 0.000000000071 -0.000000001684 0.000000001552 -0.000000007042 0.000000257460 -0.000000010870 0.000000002616 -0.000000012949 0.000000003016 0.000000000544 -0.000000010870 0.000000243242 -0.000000002361 0.000000025066 -0.000000006564 0.000000000071 0.000000002616 -0.000000002361 0.000000225067
1 1645005905.903995513916 52 53 -0.002725003716 0.004085843921 -0.000203663179 0.000790836836 0.000587671331 -0.019275453487 0.000000078783 -0.000000017322 0.000000001763 -0.000000001459 -0.000000013309 0.000000025091 -0.000000017322 0.000000012536 -0.000000000705 0.000000001676 0.000000003055 -0.000000006665 0.000000001763 -0.000000000705 0.000000012882 -0.000000007071 0.000000000738 0.000000000012 -0.000000001459 0.000000001676 -0.000000007071 0.000000258028 -0.000000012023 0.000000002543 -0.000000013309 0.000000003055 0.000000000738 -0.000000012023 0.000000244738 -0.000000002147 0.000000025091 -0.000000006665 0.000000000012 0.000000002543 -0.000000002147 0.000000228144
1 1645005906.006122350693 53 54 -0.001467286616 0.002911939063 0.001069730244 0.000570837090 0.000647669773 -0.017525510312 0.000000079128 -0.000000017240 0.000000001741 -0.000000001521 -0.000000013627 0.000000025287 -0.000000017240 0.000000012719 -0.000000000684 0.000000001673 0.000000003084 -0.000000006545 0.000000001741 -0.000000000684 0.000000012768 -0.000000007310 0.000000001027 0.000000000025 -0.000000001521 0.000000001673 -0.000000007310 0.000000260589 -0.000000013608 0.000000002539 -0.000000013627 0.000000003084 0.000000001027 -0.000000013608 0.000000247534 -0.000000002295 0.000000025287 -0.000000006545 0.000000000025 0.000000002539 -0.000000002295 0.000000231204
1 1645005906.099501609802 54 55 -0.001059218310 0.001595583380 0.001718691048 0.000383564738 0.000421259981 -0.014061465138 0.000000078834 -0.000000016986 0.000000001758 -0.000000001750 -0.000000013531 0.000000025769 -0.000000016986 0.000000012611 -0.000000000664 0.000000001714 0.000000002983 -0.000000006153 0.000000001758 -0.000000000664 0.000000012509 -0.000000007145 0.000000001254 0.000000000088 -0.000000001750 0.000000001714 -0.000000007145 0.000000263017 -0.000000014906 0.000000002634 -0.000000013531 0.000000002983 0.000000001254 -0.000000014906 0.000000249896 -0.000000002598 0.000000025769 -0.000000006153 0.000000000088 0.000000002634 -0.000000002598 0.000000233378
1 1645005906.199391126633 55 56 -0.001373475218 0.001308808338 0.000378930768 0.000479365502 0.000490820641 -0.009012238322 0.000000077943 -0.000000016768 0.000000001735 -0.000000001945 -0.000000013077 0.000000026054 -0.000000016768 0.000000012465 -0.000000000630 0.000000001711 0.000000002929 -0.000000005670 0.000000001735 -0.000000000630 0.000000012365 -0.000000007041 0.000000001545 0.000000000018 -0.000000001945 0.000000001711 -0.000000007041 0.000000261574 -0.000000015843 0.000000002953 -0.000000013077 0.000000002929 0.000000001545 -0.000000015843 0.000000248225 -0.000000002881 0.000000026054 -0.000000005670 0.000000000018 0.000000002953 -0.000000002881 0.000000232505
1 1645005906.297765493393 56 57 -0.001355900545 0.000745568120 -0.000870920659 0.000244984256 0.000234169826 -0.004721749525 0.000000077729 -0.000000016651 0.000000001590 -0.000000002229 -0.000000012620 0.000000026130 -0.000000016651 0.000000012411 -0.000000000629 0.000000001763 0.000000002808 -0.000000005332 0.000000001590 -0.000000000629 0.000000012914 -0.000000007257 0.000000001540 -0.000000000032 -0.000000002229 0.000000001763 -0.000000007257 0.000000261378 -0.000000015902 0.000000003371 -0.000000012620 0.000000002808 0.000000001540 -0.000000015902 0.000000247742 -0.000000003003 0.000000026130 -0.000000005332 -0.000000000032 0.000000003371 -0.000000003003 0.000000232061
1 1645005906.399662256241 57 58 -0.001773912220 0.001216253031 -0.001438523133 0.000093249864 0.000296808797 -0.001971036703 0.000000077865 -0.000000016538 0.000000001543 -0.000000002529 -0.000000012492 0.000000026251 -0.000000016538 0.000000012416 -0.000000000643 0.000000001937 0.000000002742 -0.000000005129 0.000000001543 -0.000000000643 0.000000013403 -0.000000007617 0.000000001310 -0.000000000089 -0.000000002529 0.000000001937 -0.000000007617 0.000000265989 -0.000000015382 0.000000003738 -0.000000012492 0.000000002742 0.000000001310 -0.000000015382 0.000000252116 -0.000000002953 0.000000026251 -0.000000005129 -0.000000000089 0.000000003738 -0.000000002953 0.000000233770
1 1645005906.492895603180 58 59 -0.002660918339 0.001015475851 -0.001383065292 0.000330318405 -0.000006555949 -0.001300286378 0.000000077810 -0.000000016434 0.000000001630 -0.000000002398 -0.000000012724 0.000000026385 -0.000000016434 0.000000012330 -0.000000000622 0.000000001970 0.000000002832 -0.000000004942 0.000000001630 -0.000000000622 0.000000013443 -0.000000008032 0.000000001316 -0.000000000234 -0.000000002398 0.000000001970 -0.000000008032 0.000000272129 -0.000000015431 0.000000004625 -0.000000012724 0.000000002832 0.000000001316 -0.000000015431 0.000000257860 -0.000000002721 0.000000026385 -0.000000004942 -0.000000000234 0.000000004625 -0.000000002721 0.000000235343
1 1645005906.599270582199 59 60 -0.004412984366 -0.001971971670 -0.001231997931 -0.000056688480 0.000101702338 -0.001115010901 0.000000077414 -0.000000016358 0.000000001582 -0.000000001830 -0.000000012780 0.000000026368 -0.000000016358 0.000000012249 -0.000000000629 0.000000001982 0.000000002761 -0.000000004929 0.000000001582 -0.000000000629 0.000000013473 -0.000000007965 0.000000001386 -0.000000000292 -0.000000001830 0.000000001982 -0.000000007965 0.000000274562 -0.000000016461 0.000000005637 -0.000000012780 0.000000002761 0.000000001386 -0.000000016461 0.000000259275 -0.000000002242 0.000000026368 -0.000000004929 -0.000000000292 0.000000005637 -0.000000002242 0.000000234454
1 1645005906.697822093964 60 61 -0.005188840029 -0.004254111564 -0.000054909271 0.000406498966 -0.000296622837 0.000416988251 0.000000077214 -0.000000016250 0.000000001521 -0.000000001261 -0.000000012771 0.000000026760 -0.000000016250 0.000000012152 -0.000000000625 0.000000001905 0.000000002698 -0.000000005164 0.000000001521 -0.000000000625 0.000000013822 -0.000000008083 0.000000001309 -0.000000000328 -0.000000001261 0.000000001905 -0.000000008083 0.000000278328 -0.000000017447 0.000000005714 -0.000000012771 0.000000002698 0.000000001309 -0.000000017447 0.000000262592 -0.000000002008 0.000000026760 -0.000000005164 -0.000000000328 0.000000005714 -0.000000002008 0.000000235536
1 1645005906.798193216324 61 62 -0.005350833950 -0.005705298333 0.000741755143 0.000130549378 -0.000347185966 0.003649959900 0.000000077631 -0.000000016247 0.000000001454 -0.000000001004 -0.000000012891 0.000000027268 -0.000000016247 0.000000012128 -0.000000000685 0.000000001939 0.000000002674 -0.000000005295 0.000000001454 -0.000000000685 0.000000014071 -0.000000008387 0.000000001361 -0.000000000365 -0.000000001004 0.000000001939 -0.000000008387 0.000000282769 -0.000000017695 0.000000005791 -0.000000012891 0.000000002674 0.000000001361 -0.000000017695 0.000000267197 -0.000000002088 0.000000027268 -0.000000005295 -0.000000000365 0.000000005791 -0.000000002088 0.000000238789
1 1645005906.897775650024 62 63 -0.003697841110 -0.006789558112 -0.000589171782 0.000044608399 -0.000455644208 0.005951637027 0.000000077672 -0.000000016231 0.000000001373 -0.000000000697 -0.000000012771 0.000000027509 -0.000000016231 0.000000012025 -0.000000000658 0.000000001829 0.000000002608 -0.000000005327 0.000000001373 -0.000000000658 0.000000013806 -0.000000008294 0.000000001746 -0.000000000347 -0.000000000697 0.000000001829 -0.000000008294 0.000000283100 -0.000000016930 0.000000006179 -0.000000012771 0.000000002608 0.000000001746 -0.000000016930 0.000000267634 -0.000000001907 0.000000027509 -0.000000005327 -0.000000000347 0.000000006179 -0.000000001907 0.000000238791
1 1645005907.000208616257 63 64 -0.002944765776 -0.006697802741 -0.003116587607 -0.000323320374 -0.000089765171 0.006392215701 0.000000077610 -0.000000016115 0.000000001489 -0.000000000549 -0.000000012506 0.000000027748 -0.000000016115 0.000000011973 -0.000000000586 0.000000001526 0.000000002609 -0.000000005268 0.000000001489 -0.000000000586 0.000000013816 -0.000000008429 0.000000001848 -0.000000000278 -0.000000000549 0.000000001526 -0.000000008429 0.000000282717 -0.000000015968 0.000000006292 -0.000000012506 0.000000002609 0.000000001848 -0.000000015968 0.000000266926 -0.000000001561 0.000000027748 -0.000000005268 -0.000000000278 0.000000006292 -0.000000001561 0.000000238503
1 1645005907.098475456238 64 65 -0.001290240390 -0.005396750920 -0.004723595682 -0.000007365578 0.000016435762 0.004075705511 0.000000077997 -0.000000016170 0.000000001561 -0.000000000791 -0.000000012603 0.000000027663 -0.000000016170 0.000000012052 -0.000000000605 0.000000001609 0.000000002641 -0.000000005184 0.000000001561 -0.000000000605 0.000000013763 -0.000000008737 0.000000001470 -0.000000000238 -0.000000000791 0.000000001609 -0.000000008737 0.000000281532 -0.000000015977 0.000000006340 -0.000000012603 0.000000002641 0.000000001470 -0.000000015977 0.000000264521 -0.000000001627 0.000000027663 -0.000000005184 -0.000000000238 0.000000006340 -0.000000001627 0.000000238811
1 1645005907.198553800583 65 66 -0.000669485805 -0.005485448301 -0.002930170548 -0.000146916616 0.000044397139 -0.000044314603 0.000000078888 -0.000000016541 0.000000001493 -0.000000000960 -0.000000013153 0.000000027258 -0.000000016541 0.000000012208 -0.000000000625 0.000000001783 0.000000002743 -0.000000005116 0.000000001493 -0.000000000625 0.000000013535 -0.000000008377 0.000000001173 -0.000000000224 -0.000000000960 0.000000001783 -0.000000008377 0.000000278934 -0.000000015798 0.000000005980 -0.000000013153 0.000000002743 0.000000001173 -0.000000015798 0.000000261815 -0.000000001742 0.000000027258 -0.000000005116 -0.000000000224 0.000000005980 -0.000000001742 0.000000238650
1 1645005907.299114227295 66 67 -0.000737675673 -0.006602658374 0.001504502005 0.000251984015 0.000332600718 -0.004637221316 0.000000078978 -0.000000016696 0.000000001539 -0.000000001083 -0.000000013528 0.000000027256 -0.000000016696 0.000000012302 -0.000000000624 0.000000001764 0.000000002889 -0.000000005225 0.000000001539 -0.000000000624 0.000000013491 -0.000000008150 0.000000001072 -0.000000000210 -0.000000001083 0.000000001764 -0.000000008150 0.000000275585 -0.000000015902 0.000000005576 -0.000000013528 0.000000002889 0.000000001072 -0.000000015902 0.000000258948 -0.000000001809 0.000000027256 -0.000000005225 -0.000000000210 0.000000005576 -0.000000001809 0.000000238197
1 1645005907.402397871017 67 68 -0.000551820100 -0.007603293394 0.003319136466 0.000085475476 0.000206125712 -0.005773773594 0.000000078821 -0.000000016809 0.000000001506 -0.000000001311 -0.000000013626 0.000000027682 -0.000000016809 0.000000012396 -0.000000000614 0.000000001766 0.000000002926 -0.000000005476 0.000000001506 -0.000000000614 0.000000013915 -0.000000008554 0.000000000776 -0.000000000165 -0.000000001311 0.000000001766 -0.000000008554 0.000000275078 -0.000000015973 0.000000005780 -0.000000013626 0.000000002926 0.000000000776 -0.000000015973 0.000000258664 -0.000000002050 0.000000027682 -0.000000005476 -0.000000000165 0.000000005780 -0.000000002050 0.000000238900
1 1645005907.500724315643 68 69 -0.000078335904 -0.007900987662 0.000767252658 0.000325260818 0.000204141643 -0.004069357072 0.000000079410 -0.000000017045 0.000000001346 -0.000000001198 -0.000000013307 0.000000027863 -0.000000017045 0.000000012459 -0.000000000602 0.000000001790 0.000000002868 -0.000000005540 0.000000001346 -0.000000000602 0.000000014320 -0.000000009106 0.000000000386 -0.000000000282 -0.000000001198 0.000000001790 -0.000000009106 0.000000277944 -0.000000015885 0.000000006351 -0.000000013307 0.000000002868 0.000000000386 -0.000000015885 0.000000260452 -0.000000002077 0.000000027863 -0.000000005540 -0.000000000282 0.000000006351 -0.000000002077 0.000000240393
1 1645005907.605340957642 69 70 0.001043055952 -0.008573117265 -0.001598483094 -0.000097387849 -0.000222906961 -0.000595407429 0.000000080055 -0.000000017438 0.000000001351 -0.000000001308 -0.000000013000 0.000000027705 -0.000000017438 0.000000012484 -0.000000000606 0.000000001915 0.000000002917 -0.000000005219 0.000000001351 -0.000000000606 0.000000014067 -0.000000009196 0.000000000300 -0.000000000458 -0.000000001308 0.000000001915 -0.000000009196 0.000000278309 -0.000000015861 0.000000006592 -0.000000013000 0.000000002917 0.000000000300 -0.000000015861 0.000000260164 -0.000000002000 0.000000027705 -0.000000005219 -0.000000000458 0.000000006592 -0.000000002000 0.000000240578
1 1645005907.703968524933 70 71 0.001635189256 -0.007661342710 -0.000611987754 0.000195863477 0.000220085415 0.000455212442 0.000000081616 -0.000000018000 0.000000001433 -0.000000001641 -0.000000013192 0.000000027466 -0.000000018000 0.000000012653 -0.000000000591 0.000000001940 0.000000003029 -0.000000005006 0.000000001433 -0.000000000591 0.000000014157 -0.000000009101 0.000000000295 -0.000000000473 -0.000000001641 0.000000001940 -0.000000009101 0.000000278289 -0.000000015793 0.000000006721 -0.000000013192 0.000000003029 0.000000000295 -0.000000015793 0.000000260033 -0.000000002174 0.000000027466 -0.000000005006 -0.000000000473 0.000000006721 -0.000000002174 0.000000241348
1 1645005907.804278135300 71 72 0.001549220998 -0.007393823942 0.001064792531 -0.000061245968 0.000265809889 -0.000238367926 0.000000084430 -0.000000018916 0.000000001290 -0.000000001506 -0.000000013499 0.000000026763 -0.000000018916 0.000000013096 -0.000000000595 0.000000001859 0.000000003095 -0.000000004652 0.000000001290 -0.000000000595 0.000000014779 -0.000000009309 0.000000000116 -0.000000000490 -0.000000001506 0.000000001859 -0.000000009309 0.000000280877 -0.000000016432 0.000000006827 -0.000000013499 0.000000003095 0.000000000116 -0.000000016432 0.000000260406 -0.000000002092 0.000000026763 -0.000000004652 -0.000000000490 0.000000006827 -0.000000002092 0.000000242565
1 1645005907.903728008270 72 73 0.002219839781 -0.006729865633 0.001166665107 0.000120456909 0.000039397030 -0.001195289279 0.000000086968 -0.000000019811 0.000000001166 -0.000000001441 -0.000000013646 0.000000025791 -0.000000019811 0.000000013556 -0.000000000638 0.000000001888 0.000000003216 -0.000000004211 0.000000001166 -0.000000000638 0.000000015013 -0.000000009450 -0.000000000298 -0.000000000604 -0.000000001441 0.000000001888 -0.000000009450 0.000000282683 -0.000000017245 0.000000006808 -0.000000013646 0.000000003216 -0.000000000298 -0.000000017245 0.000000258711 -0.000000001579 0.000000025791 -0.000000004211 -0.000000000604 0.000000006808 -0.000000001579 0.000000241863
1 1645005908.003634452820 73 74 0.003125083194 -0.006057082466 0.000100060086 -0.000042155704 -0.000130882188 -0.000945982049 0.000000088087 -0.000000020274 0.000000001242 -0.000000001785 -0.000000013450 0.000000024803 -0.000000020274 0.000000013671 -0.000000000680 0.000000002063 0.000000003299 -0.000000004076 0.000000001242 -0.000000000680 0.000000014959 -0.000000009260 -0.000000000646 -0.000000000747 -0.000000001785 0.000000002063 -0.000000009260 0.000000284481 -0.000000018051 0.000000007211 -0.000000013450 0.000000003299 -0.000000000646 -0.000000018051 0.000000257869 -0.000000000830 0.000000024803 -0.000000004076 -0.000000000747 0.000000007211 -0.000000000830 0.000000240461
1 1645005908.103469848633 74 75 0.005239355570 -0.006690197943 -0.001346041897 0.000002110025 0.000383511679 0.000870726890 0.000000088096 -0.000000020346 0.000000001268 -0.000000002250 -0.000000013524 0.000000023906 -0.000000020346 0.000000013591 -0.000000000702 0.000000002211 0.000000003331 -0.000000003807 0.000000001268 -0.000000000702 0.000000014950 -0.000000008801 -0.000000000587 -0.000000000765 -0.000000002250 0.000000002211 -0.000000008801 0.000000284454 -0.000000018287 0.000000007836 -0.000000013524 0.000000003331 -0.000000000587 -0.000000018287 0.000000257037 -0.000000000613 0.000000023906 -0.000000003807 -0.000000000765 0.000000007836 -0.000000000613 0.000000238618
1 1645005908.202678442001 75 76 0.006817804937 -0.009726863760 -0.000518636839 -0.000056178504 -0.000656898114 0.002883550854 0.000000088802 -0.000000020528 0.000000001285 -0.000000002483 -0.000000014210 0.000000023286 -0.000000020528 0.000000013726 -0.000000000706 0.000000002292 0.000000003404 -0.000000003359 0.000000001285 -0.000000000706 0.000000014784 -0.000000008320 -0.000000000291 -0.000000000649 -0.000000002483 0.000000002292 -0.000000008320 0.000000283431 -0.000000018165 0.000000008157 -0.000000014210 0.000000003404 -0.000000000291 -0.000000018165 0.000000255778 -0.000000000733 0.000000023286 -0.000000003359 -0.000000000649 0.000000008157 -0.000000000733 0.000000239003
1 1645005908.303349256516 76 77 0.008880185968 -0.011072420154 0.002005117294 0.000364039331 0.000621317606 0.001463440780 0.000000089528 -0.000000020704 0.000000001309 -0.000000002811 -0.000000014365 0.000000022726 -0.000000020704 0.000000013849 -0.000000000704 0.000000002410 0.000000003408 -0.000000003006 0.000000001309 -0.000000000704 0.000000014662 -0.000000008440 -0.000000000182 -0.000000000582 -0.000000002811 0.000000002410 -0.000000008440 0.000000282898 -0.000000018035 0.000000008233 -0.000000014365 0.000000003408 -0.000000000182 -0.000000018035 0.000000254217 -0.000000000493 0.000000022726 -0.000000003006 -0.000000000582 0.000000008233 -0.000000000493 0.000000239589
1 1645005908.403426647186 77 78 0.013214057998 -0.010022380808 0.003548089026 -0.000420898660 -0.000687846562 0.004288662412 0.000000090426 -0.000000021194 0.000000001135 -0.000000003300 -0.000000014313 0.000000022653 -0.000000021194 0.000000013961 -0.000000000638 0.000000002484 0.000000003448 -0.000000002827 0.000000001135 -0.000000000638 0.000000014468 -0.000000009028 -0.000000000586 -0.000000000561 -0.000000003300 0.000000002484 -0.000000009028 0.000000283166 -0.000000017970 0.000000008066 -0.000000014313 0.000000003448 -0.000000000586 -0.000000017970 0.000000253551 -0.000000000440 0.000000022653 -0.000000002827 -0.000000000561 0.000000008066 -0.000000000440 0.000000240482
1 1645005908.504360437393 78 79 0.020180381727 -0.009462892108 0.001955139025 0.000042094269 0.000191650148 0.004876347524 0.000000091477 -0.000000021398 0.000000001260 -0.000000003754 -0.000000014396 0.000000022564 -0.000000021398 0.000000014143 -0.000000000654 0.000000002576 0.000000003359 -0.000000002684 0.000000001260 -0.000000000654 0.000000014202 -0.000000008908 -0.000000001365 -0.000000000321 -0.000000003754 0.000000002576 -0.000000008908 0.000000283742 -0.000000018036 0.000000007708 -0.000000014396 0.000000003359 -0.000000001365 -0.000000018036 0.000000252709 -0.000000000672 0.000000022564 -0.000000002684 -0.000000000321 0.000000007708 -0.000000000672 0.000000242092
1 1645005908.602572441101 79 80 0.027583841746 -0.007492189755 -0.000358945670 -0.000316065525 0.000410807340 0.004627049462 0.000000091890 -0.000000021219 0.000000001348 -0.000000004184 -0.000000014661 0.000000022474 -0.000000021219 0.000000014196 -0.000000000667 0.000000002818 0.000000003383 -0.000000002589 0.000000001348 -0.000000000667 0.000000014612 -0.000000008669 -0.000000001276 -0.000000000350 -0.000000004184 0.000000002818 -0.000000008669 0.000000284516 -0.000000017822 0.000000007904 -0.000000014661 0.000000003383 -0.000000001276 -0.000000017822 0.000000252238 -0.000000001096 0.000000022474 -0.000000002589 -0.000000000350 0.000000007904 -0.000000001096 0.000000242589
1 1645005908.702723264694 80 81 0.037527173709 -0.002432558917 0.000584410457 -0.000295924796 0.000172057833 0.004510494708 0.000000092503 -0.000000021428 0.000000001442 -0.000000004439 -0.000000015161 0.000000022444 -0.000000021428 0.000000014211 -0.000000000702 0.000000002984 0.000000003573 -0.000000002087 0.000000001442 -0.000000000702 0.000000014735 -0.000000008471 -0.000000001226 -0.000000000479 -0.000000004439 0.000000002984 -0.000000008471 0.000000283202 -0.000000016932 0.000000008633 -0.000000015161 0.000000003573 -0.000000001226 -0.000000016932 0.000000250551 -0.000000001604 0.000000022444 -0.000000002087 -0.000000000479 0.000000008633 -0.000000001604 0.000000241814
1 1645005908.805076837540 81 82 0.044756539731 -0.000661973435 0.002178494075 -0.000609676361 0.000455606665 0.001515229361 0.000000093517 -0.000000021845 0.000000001449 -0.000000004558 -0.000000015501 0.000000022061 -0.000000021845 0.000000014347 -0.000000000736 0.000000003042 0.000000003685 -0.000000001671 0.000000001449 -0.000000000736 0.000000014651 -0.000000008202 -0.000000002017 -0.000000000480 -0.000000004558 0.000000003042 -0.000000008202 0.000000281630 -0.000000016156 0.000000009346 -0.000000015501 0.000000003685 -0.000000002017 -0.000000016156 0.000000248540 -0.000000001725 0.000000022061 -0.000000001671 -0.000000000480 0.000000009346 -0.000000001725 0.000000241443
1 1645005908.902604103088 82 83 0.045146081256 -0.002522614562 0.003315811413 0.000215551457 0.001434294478 -0.002791558929 0.000000095523 -0.000000022658 0.000000001480 -0.000000004967 -0.000000015958 0.000000021670 -0.000000022658 0.000000014741 -0.000000000778 0.000000003208 0.000000003828 -0.000000001330 0.000000001480 -0.000000000778 0.000000014730 -0.000000007858 -0.000000002516 -0.000000000414 -0.000000004967 0.000000003208 -0.000000007858 0.000000281929 -0.000000016137 0.000000009579 -0.000000015958 0.000000003828 -0.000000002516 -0.000000016137 0.000000249394 -0.000000001587 0.000000021670 -0.000000001330 -0.000000000414 0.000000009579 -0.000000001587 0.000000243425
1 1645005909.003700494766 83 84 0.050021001197 -0.004220963579 0.004624075743 0.000077137405 0.000861877284 -0.003931105805 0.000000097156 -0.000000023529 0.000000001589 -0.000000005276 -0.000000016382 0.000000021184 -0.000000023529 0.000000015119 -0.000000000815 0.000000003220 0.000000004081 -0.000000000901 0.000000001589 -0.000000000815 0.000000014639 -0.000000007417 -0.000000002865 -0.000000000427 -0.000000005276 0.000000003220 -0.000000007417 0.000000278551 -0.000000015767 0.000000008985 -0.000000016382 0.000000004081 -0.000000002865 -0.000000015767 0.000000248181 -0.000000001156 0.000000021184 -0.000000000901 -0.000000000427 0.000000008985 -0.000000001156 0.000000244236
1 1645005909.105985641479 84 85 0.055306161593 -0.005644162807 0.003857391803 0.000176010803 -0.000406336505 -0.002605005858 0.000000098538 -0.000000024037 0.000000001724 -0.000000005135 -0.000000016925 0.000000020936 -0.000000024037 0.000000015386 -0.000000000857 0.000000002936 0.000000004319 -0.000000000573 0.000000001724 -0.000000000857 0.000000014932 -0.000000007133 -0.000000003111 -0.000000000413 -0.000000005135 0.000000002936 -0.000000007133 0.000000274491 -0.000000015304 0.000000008087 -0.000000016925 0.000000004319 -0.000000003111 -0.000000015304 0.000000247261 -0.000000000996 0.000000020936 -0.000000000573 -0.000000000413 0.000000008087 -0.000000000996 0.000000244577
1 1645005909.204295873642 85 86 0.053724760856 -0.007867742945 0.002362917646 0.000765560740 0.000896643338 -0.004875595418 0.000000100571 -0.000000024471 0.000000001820 -0.000000004997 -0.000000017493 0.000000020789 -0.000000024471 0.000000015583 -0.000000000907 0.000000002811 0.000000004425 -0.000000000103 0.000000001820 -0.000000000907 0.000000014997 -0.000000007018 -0.000000003853 -0.000000000322 -0.000000004997 0.000000002811 -0.000000007018 0.000000274574 -0.000000014663 0.000000007432 -0.000000017493 0.000000004425 -0.000000003853 -0.000000014663 0.000000250668 -0.000000000903 0.000000020789 -0.000000000103 -0.000000000322 0.000000007432 -0.000000000903 0.000000246442
1 1645005909.301892518997 86 87 0.052598742656 -0.009586402032 -0.001683730482 0.000100902220 0.000694494544 -0.005642354434 0.000000101572 -0.000000024847 0.000000001621 -0.000000004490 -0.000000017492 0.000000020848 -0.000000024847 0.000000015608 -0.000000000748 0.000000002623 0.000000004392 0.000000000187 0.000000001621 -0.000000000748 0.000000014697 -0.000000007106 -0.000000005019 -0.000000000290 -0.000000004490 0.000000002623 -0.000000007106 0.000000274211 -0.000000014372 0.000000006733 -0.000000017492 0.000000004392 -0.000000005019 -0.000000014372 0.000000253261 -0.000000001008 0.000000020848 0.000000000187 -0.000000000290 0.000000006733 -0.000000001008 0.000000245747
1 1645005909.403104543686 87 88 0.056969463690 -0.013394491540 -0.004907688527 0.000313438663 -0.000019403912 -0.004039402462 0.000000101997 -0.000000025070 0.000000001444 -0.000000003922 -0.000000017659 0.000000021025 -0.000000025070 0.000000015563 -0.000000000684 0.000000002332 0.000000004455 0.000000000663 0.000000001444 -0.000000000684 0.000000015000 -0.000000007663 -0.000000005985 -0.000000000282 -0.000000003922 0.000000002332 -0.000000007663 0.000000273327 -0.000000013785 0.000000006215 -0.000000017659 0.000000004455 -0.000000005985 -0.000000013785 0.000000254655 -0.000000001345 0.000000021025 0.000000000663 -0.000000000282 0.000000006215 -0.000000001345 0.000000243873
1 1645005909.501364469528 88 89 0.059762523379 -0.018567501801 -0.005975427554 0.000327232703 -0.000227548183 -0.003605858449 0.000000103041 -0.000000025390 0.000000001364 -0.000000003806 -0.000000017859 0.000000021654 -0.000000025390 0.000000015487 -0.000000000758 0.000000002227 0.000000004664 0.000000001355 0.000000001364 -0.000000000758 0.000000015660 -0.000000008246 -0.000000006861 -0.000000000445 -0.000000003806 0.000000002227 -0.000000008246 0.000000275936 -0.000000013446 0.000000006125 -0.000000017859 0.000000004664 -0.000000006861 -0.000000013446 0.000000258726 -0.000000001189 0.000000021654 0.000000001355 -0.000000000445 0.000000006125 -0.000000001189 0.000000245723
1 1645005909.601603746414 89 90 0.063283735135 -0.025948706002 -0.004113490990 0.000699255821 0.000279829928 -0.004494526640 0.000000104093 -0.000000025791 0.000000001237 -0.000000003691 -0.000000017610 0.000000022878 -0.000000025791 0.000000015579 -0.000000000780 0.000000002254 0.000000004734 0.000000001701 0.000000001237 -0.000000000780 0.000000016004 -0.000000008598 -0.000000007978 -0.000000000633 -0.000000003691 0.000000002254 -0.000000008598 0.000000279345 -0.000000013365 0.000000006383 -0.000000017610 0.000000004734 -0.000000007978 -0.000000013365 0.000000262863 -0.000000000672 0.000000022878 0.000000001701 -0.000000000633 0.000000006383 -0.000000000672 0.000000249192
1 1645005909.706259727478 90 91 0.064392495618 -0.030468368848 0.003371720855 0.000963763803 0.001428163796 -0.005769357514 0.000000105109 -0.000000026290 0.000000001203 -0.000000003464 -0.000000017388 0.000000024107 -0.000000026290 0.000000015676 -0.000000000845 0.000000002355 0.000000004759 0.000000001923 0.000000001203 -0.000000000845 0.000000016121 -0.000000008819 -0.000000009190 -0.000000000794 -0.000000003464 0.000000002355 -0.000000008819 0.000000281196 -0.000000012993 0.000000007076 -0.000000017388 0.000000004759 -0.000000009190 -0.000000012993 0.000000265075 -0.000000000134 0.000000024107 0.000000001923 -0.000000000794 0.000000007076 -0.000000000134 0.000000251988
1 1645005909.802580833435 91 92 0.062365238940 -0.025844183621 0.009407834302 0.000193492681 -0.000172715186 -0.001464084852 0.000000108795 -0.000000027506 0.000000001224 -0.000000003570 -0.000000017752 0.000000025071 -0.000000027506 0.000000016130 -0.000000000716 0.000000002314 0.000000004730 0.000000002329 0.000000001224 -0.000000000716 0.000000015944 -0.000000008872 -0.000000010128 -0.000000000541 -0.000000003570 0.000000002314 -0.000000008872 0.000000285835 -0.000000013072 0.000000007112 -0.000000017752 0.000000004730 -0.000000010128 -0.000000013072 0.000000269332 -0.000000000648 0.000000025071 0.000000002329 -0.000000000541 0.000000007112 -0.000000000648 0.000000257045
1 1645005909.903105735779 92 93 0.068784629133 -0.022584999076 0.007314965158 -0.000173602250 -0.000981733227 0.004132128312 0.000000111499 -0.000000028105 0.000000001268 -0.000000003943 -0.000000017982 0.000000024931 -0.000000028105 0.000000016532 -0.000000000666 0.000000002326 0.000000004775 0.000000003407 0.000000001268 -0.000000000666 0.000000015946 -0.000000009222 -0.000000011103 -0.000000000468 -0.000000003943 0.000000002326 -0.000000009222 0.000000288991 -0.000000012266 0.000000007214 -0.000000017982 0.000000004775 -0.000000011103 -0.000000012266 0.000000270553 -0.000000000807 0.000000024931 0.000000003407 -0.000000000468 0.000000007214 -0.000000000807 0.000000260827
1 1645005910.002853631973 93 94 0.064100403759 -0.021302988329 0.003328486309 -0.000115742040 0.000269762022 0.004338534235 0.000000113187 -0.000000028301 0.000000001447 -0.000000004451 -0.000000017932 0.000000024371 -0.000000028301 0.000000016578 -0.000000000743 0.000000002509 0.000000004720 0.000000004613 0.000000001447 -0.000000000743 0.000000015755 -0.000000009213 -0.000000011391 -0.000000000441 -0.000000004451 0.000000002509 -0.000000009213 0.000000291379 -0.000000012472 0.000000007953 -0.000000017932 0.000000004720 -0.000000011391 -0.000000012472 0.000000268086 -0.000000000754 0.000000024371 0.000000004613 -0.000000000441 0.000000007953 -0.000000000754 0.000000263152
1 1645005910.102889776230 94 95 0.058187415660 -0.021959187640 -0.002395190107 -0.000230382660 0.000479950528 0.004722778418 0.000000114411 -0.000000028797 0.000000001597 -0.000000005011 -0.000000018281 0.000000023886 -0.000000028797 0.000000016614 -0.000000000790 0.000000002676 0.000000004786 0.000000005410 0.000000001597 -0.000000000790 0.000000015406 -0.000000009290 -0.000000011420 -0.000000000347 -0.000000005011 0.000000002676 -0.000000009290 0.000000295984 -0.000000013121 0.000000008785 -0.000000018281 0.000000004786 -0.000000011420 -0.000000013121 0.000000268118 -0.000000001232 0.000000023886 0.000000005410 -0.000000000347 0.000000008785 -0.000000001232 0.000000266828
1 1645005910.203362464905 95 96 0.056619890717 -0.019994351111 -0.005006673376 -0.000078075128 -0.001163126359 0.005855695866 0.000000113848 -0.000000028816 0.000000001425 -0.000000004988 -0.000000018908 0.000000023457 -0.000000028816 0.000000016671 -0.000000000790 0.000000002758 0.000000005082 0.000000006329 0.000000001425 -0.000000000790 0.000000015387 -0.000000009472 -0.000000012713 -0.000000000564 -0.000000004988 0.000000002758 -0.000000009472 0.000000298099 -0.000000012668 0.000000009794 -0.000000018908 0.000000005082 -0.000000012713 -0.000000012668 0.000000271956 -0.000000001334 0.000000023457 0.000000006329 -0.000000000564 0.000000009794 -0.000000001334 0.000000269624
1 1645005910.304825067520 96 97 0.061086566852 -0.020286198113 -0.004843598885 0.000071324355 -0.000601247565 0.004021009982 0.000000117880 -0.000000029623 0.000000001330 -0.000000004609 -0.000000019305 0.000000024344 -0.000000029623 0.000000017038 -0.000000000772 0.000000002859 0.000000005198 0.000000006765 0.000000001330 -0.000000000772 0.000000015981 -0.000000010001 -0.000000014626 -0.000000000763 -0.000000004609 0.000000002859 -0.000000010001 0.000000301426 -0.000000011273 0.000000010740 -0.000000019305 0.000000005198 -0.000000014626 -0.000000011273 0.000000276684 -0.000000001301 0.000000024344 0.000000006765 -0.000000000763 0.000000010740 -0.000000001301 0.000000271470
1 1645005910.406254529953 97 98 0.068850355530 -0.020207965588 -0.000165344613 -0.000090050301 -0.000186271132 0.000092130259 0.000000120621 -0.000000030569 0.000000001323 -0.000000004733 -0.000000019353 0.000000024043 -0.000000030569 0.000000017239 -0.000000000763 0.000000002867 0.000000005258 0.000000007204 0.000000001323 -0.000000000763 0.000000016177 -0.000000010171 -0.000000016172 -0.000000000720 -0.000000004733 0.000000002867 -0.000000010171 0.000000306276 -0.000000010789 0.000000010460 -0.000000019353 0.000000005258 -0.000000016172 -0.000000010789 0.000000281858 -0.000000001403 0.000000024043 0.000000007204 -0.000000000720 0.000000010460 -0.000000001403 0.000000273593
1 1645005910.503567218781 98 99 0.074162405982 -0.018493810987 0.006239807671 -0.000159910511 0.000247959785 -0.004015256391 0.000000119575 -0.000000030905 0.000000001301 -0.000000005274 -0.000000019174 0.000000021991 -0.000000030905 0.000000017327 -0.000000000670 0.000000002923 0.000000005171 0.000000008213 0.000000001301 -0.000000000670 0.000000015751 -0.000000009557 -0.000000017503 -0.000000000420 -0.000000005274 0.000000002923 -0.000000009557 0.000000310028 -0.000000012146 0.000000010051 -0.000000019174 0.000000005171 -0.000000017503 -0.000000012146 0.000000288635 -0.000000001805 0.000000021991 0.000000008213 -0.000000000420 0.000000010051 -0.000000001805 0.000000275801
1 1645005910.606616973877 99 100 0.080910102851 -0.020164208266 0.008661834846 0.000071976494 0.000898942665 -0.008152706685 0.000000119264 -0.000000031192 0.000000001273 -0.000000006002 -0.000000018615 0.000000020354 -0.000000031192 0.000000017400 -0.000000000693 0.000000003292 0.000000005053 0.000000009012 0.000000001273 -0.000000000693 0.000000015378 -0.000000009114 -0.000000018448 -0.000000000406 -0.000000006002 0.000000003292 -0.000000009114 0.000000310497 -0.000000014070 0.000000010804 -0.000000018615 0.000000005053 -0.000000018448 -0.000000014070 0.000000292764 -0.000000001847 0.000000020354 0.000000009012 -0.000000000406 0.000000010804 -0.000000001847 0.000000274724
1 1645005910.704123735428 100 101 0.075961027781 -0.021808661780 0.005934638148 0.000450549674 0.001441942574 -0.012050260842 0.000000119262 -0.000000031760 0.000000001275 -0.000000006676 -0.000000018333 0.000000018441 -0.000000031760 0.000000017569 -0.000000000766 0.000000003498 0.000000005201 0.000000009821 0.000000001275 -0.000000000766 0.000000015022 -0.000000009165 -0.000000018987 -0.000000000583 -0.000000006676 0.000000003498 -0.000000009165 0.000000310167 -0.000000014428 0.000000011047 -0.000000018333 0.000000005201 -0.000000018987 -0.000000014428 0.000000295633 -0.000000001178 0.000000018441 0.000000009821 -0.000000000583 0.000000011047 -0.000000001178 0.000000274189
1 1645005910.803392648697 101 102 0.078482673806 -0.023461910881 0.002857169426 0.000627784303 0.000447074489 -0.011957121987 0.000000119092 -0.000000032121 0.000000001304 -0.000000007124 -0.000000018655 0.000000017333 -0.000000032121 0.000000017789 -0.000000000790 0.000000003564 0.000000005412 0.000000010492 0.000000001304 -0.000000000790 0.000000015124 -0.000000009379 -0.000000019322 -0.000000000660 -0.000000007124 0.000000003564 -0.000000009379 0.000000308153 -0.000000013438 0.000000010595 -0.000000018655 0.000000005412 -0.000000019322 -0.000000013438 0.000000297079 -0.000000000659 0.000000017333 0.000000010492 -0.000000000660 0.000000010595 -0.000000000659 0.000000273340
1 1645005910.902909994125 102 103 0.083189939682 -0.023490784963 -0.003117914173 0.000480762492 0.000068118626 -0.007186591478 0.000000118524 -0.000000032013 0.000000001297 -0.000000007106 -0.000000018773 0.000000016705 -0.000000032013 0.000000017971 -0.000000000795 0.000000003426 0.000000005515 0.000000011670 0.000000001297 -0.000000000795 0.000000015444 -0.000000009493 -0.000000020369 -0.000000000697 -0.000000007106 0.000000003426 -0.000000009493 0.000000306218 -0.000000012861 0.000000010051 -0.000000018773 0.000000005515 -0.000000020369 -0.000000012861 0.000000299271 -0.000000000380 0.000000016705 0.000000011670 -0.000000000697 0.000000010051 -0.000000000380 0.000000272341
1 1645005911.004977464676 103 104 0.087315384998 -0.025157189183 -0.000393238317 0.000391193097 0.000467548951 -0.002126631756 0.000000118945 -0.000000031979 0.000000001227 -0.000000007043 -0.000000019158 0.000000016413 -0.000000031979 0.000000018203 -0.000000000668 0.000000003217 0.000000005547 0.000000013380 0.000000001227 -0.000000000668 0.000000015676 -0.000000009973 -0.000000022149 -0.000000000580 -0.000000007043 0.000000003217 -0.000000009973 0.000000309925 -0.000000012331 0.000000009781 -0.000000019158 0.000000005547 -0.000000022149 -0.000000012331 0.000000306142 -0.000000000590 0.000000016413 0.000000013380 -0.000000000580 0.000000009781 -0.000000000590 0.000000276867
1 1645005911.101893663406 104 105 0.084323245144 -0.023813002632 0.014804471985 0.000199032956 0.000054108090 -0.000198912619 0.000000120217 -0.000000032143 0.000000001107 -0.000000007075 -0.000000019378 0.000000016191 -0.000000032143 0.000000018424 -0.000000000637 0.000000003358 0.000000005540 0.000000015177 0.000000001107 -0.000000000637 0.000000015707 -0.000000010434 -0.000000024174 -0.000000000604 -0.000000007075 0.000000003358 -0.000000010434 0.000000316238 -0.000000012013 0.000000010568 -0.000000019378 0.000000005540 -0.000000024174 -0.000000012013 0.000000316136 -0.000000000812 0.000000016191 0.000000015177 -0.000000000604 0.000000010568 -0.000000000812 0.000000285489
1 1645005911.202757835388 105 106 0.091890144553 -0.022457558791 0.017747265772 -0.000060833543 -0.000571579936 0.006619650214 0.000000121531 -0.000000032560 0.000000000738 -0.000000006643 -0.000000017693 0.000000015251 -0.000000032560 0.000000018747 -0.000000000671 0.000000003314 0.000000005222 0.000000017170 0.000000000738 -0.000000000671 0.000000016021 -0.000000011220 -0.000000026178 -0.000000000837 -0.000000006643 0.000000003314 -0.000000011220 0.000000321155 -0.000000010921 0.000000011729 -0.000000017693 0.000000005222 -0.000000026178 -0.000000010921 0.000000322863 -0.000000000229 0.000000015251 0.000000017170 -0.000000000837 0.000000011729 -0.000000000229 0.000000293530
1 1645005911.304598569870 106 107 0.099315592633 -0.013707866919 0.008929200938 -0.000593510612 0.000431991390 0.006542184812 0.000000122613 -0.000000032815 0.000000000312 -0.000000006490 -0.000000015355 0.000000013258 -0.000000032815 0.000000018837 -0.000000000526 0.000000003118 0.000000004545 0.000000018930 0.000000000312 -0.000000000526 0.000000016756 -0.000000012375 -0.000000028786 -0.000000000787 -0.000000006490 0.000000003118 -0.000000012375 0.000000326678 -0.000000008324 0.000000013042 -0.000000015355 0.000000004545 -0.000000028786 -0.000000008324 0.000000328892 0.000000000490 0.000000013258 0.000000018930 -0.000000000787 0.000000013042 0.000000000490 0.000000298943
1 1645005911.402243375778 107 108 0.104785384813 -0.008105171994 -0.002214641892 -0.000860946379 -0.000993392829 0.010948470463 0.000000123089 -0.000000033137 0.000000000099 -0.000000006942 -0.000000014277 0.000000011341 -0.000000033137 0.000000019181 -0.000000000321 0.000000003202 0.000000004018 0.000000020680 0.000000000099 -0.000000000321 0.000000016999 -0.000000012404 -0.000000030745 -0.000000000434 -0.000000006942 0.000000003202 -0.000000012404 0.000000331673 -0.000000007654 0.000000013557 -0.000000014277 0.000000004018 -0.000000030745 -0.000000007654 0.000000336509 0.000000000255 0.000000011341 0.000000020680 -0.000000000434 0.000000013557 0.000000000255 0.000000304958
1 1645005911.504294157028 108 109 0.115689898818 -0.005982363005 -0.006109069161 -0.000726627622 0.000973576912 0.010908783105 0.000000124858 -0.000000033368 0.000000000274 -0.000000007206 -0.000000014118 0.000000010002 -0.000000033368 0.000000019303 -0.000000000398 0.000000003340 0.000000003987 0.000000022318 0.000000000274 -0.000000000398 0.000000016946 -0.000000012012 -0.000000031995 -0.000000000446 -0.000000007206 0.000000003340 -0.000000012012 0.000000330527 -0.000000006532 0.000000013696 -0.000000014118 0.000000003987 -0.000000031995 -0.000000006532 0.000000339820 0.000000000103 0.000000010002 0.000000022318 -0.000000000446 0.000000013696 0.000000000103 0.000000309344
1 1645005911.603650331497 109 110 0.112144073030 -0.007022975029 0.001012219037 -0.001163101342 0.000302771644 0.009647953042 0.000000124674 -0.000000033654 0.000000000385 -0.000000008009 -0.000000015177 0.000000007700 -0.000000033654 0.000000019363 -0.000000000509 0.000000003826 0.000000004570 0.000000023824 0.000000000385 -0.000000000509 0.000000016545 -0.000000011791 -0.000000032496 -0.000000000820 -0.000000008009 0.000000003826 -0.000000011791 0.000000328802 -0.000000004861 0.000000014409 -0.000000015177 0.000000004570 -0.000000032496 -0.000000004861 0.000000338575 0.000000000638 0.000000007700 0.000000023824 -0.000000000820 0.000000014409 0.000000000638 0.000000314003
1 1645005911.705323696136 110 111 0.108334202057 -0.006199322902 0.015439608211 -0.000499640517 0.000622717034 0.005115544113 0.000000121420 -0.000000033136 0.000000000547 -0.000000008867 -0.000000016030 0.000000005761 -0.000000033136 0.000000019118 -0.000000000490 0.000000003998 0.000000004882 0.000000025082 0.000000000547 -0.000000000490 0.000000015937 -0.000000011322 -0.000000032770 -0.000000000779 -0.000000008867 0.000000003998 -0.000000011322 0.000000326263 -0.000000003896 0.000000014707 -0.000000016030 0.000000004882 -0.000000032770 -0.000000003896 0.000000334604 0.000000000414 0.000000005761 0.000000025082 -0.000000000779 0.000000014707 0.000000000414 0.000000319533
1 1645005911.805619001389 111 112 0.105563859350 -0.007445366800 0.017090699054 -0.000396553302 -0.000236939023 0.001638354551 0.000000119495 -0.000000032859 0.000000000407 -0.000000008551 -0.000000014394 0.000000004296 -0.000000032859 0.000000019006 -0.000000000444 0.000000003710 0.000000004531 0.000000026291 0.000000000407 -0.000000000444 0.000000015977 -0.000000011729 -0.000000035072 -0.000000000757 -0.000000008551 0.000000003710 -0.000000011729 0.000000324713 -0.000000002118 0.000000014718 -0.000000014394 0.000000004531 -0.000000035072 -0.000000002118 0.000000339187 0.000000000583 0.000000004296 0.000000026291 -0.000000000757 0.000000014718 0.000000000583 0.000000324775
1 1645005911.905131578445 112 113 0.105669045682 -0.010232842109 0.007581705384 0.000117560962 -0.000845623252 -0.002154597809 0.000000117976 -0.000000032837 0.000000000325 -0.000000008707 -0.000000012159 0.000000002537 -0.000000032837 0.000000019001 -0.000000000403 0.000000003735 0.000000003954 0.000000027237 0.000000000325 -0.000000000403 0.000000015480 -0.000000011530 -0.000000035970 -0.000000000732 -0.000000008707 0.000000003735 -0.000000011530 0.000000324418 -0.000000003537 0.000000014543 -0.000000012159 0.000000003954 -0.000000035970 -0.000000003537 0.000000343391 0.000000000796 0.000000002537 0.000000027237 -0.000000000732 0.000000014543 0.000000000796 0.000000327307
1 1645005912.005542993546 113 114 0.105408723509 -0.006257586189 -0.004647377317 -0.000008164249 -0.000462734466 -0.006034679601 0.000000115866 -0.000000032739 0.000000000255 -0.000000008904 -0.000000010564 0.000000001256 -0.000000032739 0.000000018798 -0.000000000420 0.000000003723 0.000000003731 0.000000027559 0.000000000255 -0.000000000420 0.000000015409 -0.000000012009 -0.000000036888 -0.000000000887 -0.000000008904 0.000000003723 -0.000000012009 0.000000321068 -0.000000000786 0.000000014100 -0.000000010564 0.000000003731 -0.000000036888 -0.000000000786 0.000000345395 0.000000001508 0.000000001256 0.000000027559 -0.000000000887 0.000000014100 0.000000001508 0.000000326200
1 1645005912.104353666306 114 115 0.104312051462 -0.005788696135 -0.001086666677 0.000673600379 0.002056576062 -0.010721959912 0.000000113948 -0.000000032429 0.000000000239 -0.000000008558 -0.000000009973 0.000000000000 -0.000000032429 0.000000018879 -0.000000000450 0.000000003631 0.000000003684 0.000000029056 0.000000000239 -0.000000000450 0.000000015680 -0.000000012374 -0.000000038521 -0.000000001026 -0.000000008558 0.000000003631 -0.000000012374 0.000000316878 0.000000003031 0.000000013903 -0.000000009973 0.000000003684 -0.000000038521 0.000000003031 0.000000349891 0.000000002089 0.000000000000 0.000000029056 -0.000000001026 0.000000013903 0.000000002089 0.000000329833
1 1645005912.204403162003 115 116 0.107279768307 -0.009811669098 0.013827848427 0.000095745570 -0.000565487976 -0.008974907831 0.000000111490 -0.000000032117 0.000000000243 -0.000000008340 -0.000000010631 -0.000000000159 -0.000000032117 0.000000018992 -0.000000000434 0.000000003780 0.000000003786 0.000000030191 0.000000000243 -0.000000000434 0.000000014981 -0.000000011345 -0.000000037764 -0.000000000975 -0.000000008340 0.000000003780 -0.000000011345 0.000000310920 0.000000002683 0.000000013634 -0.000000010631 0.000000003786 -0.000000037764 0.000000002683 0.000000348113 0.000000001943 -0.000000000159 0.000000030191 -0.000000000975 0.000000013634 0.000000001943 0.000000334016
1 1645005912.304095268250 116 117 0.106183988090 -0.010713891373 0.022572940623 0.000391186142 0.000422801729 -0.011307080283 0.000000109508 -0.000000031579 0.000000000488 -0.000000007880 -0.000000010858 0.000000000029 -0.000000031579 0.000000018604 -0.000000000518 0.000000003456 0.000000003941 0.000000030323 0.000000000488 -0.000000000518 0.000000014084 -0.000000010751 -0.000000036936 -0.000000001088 -0.000000007880 0.000000003456 -0.000000010751 0.000000305213 0.000000003878 0.000000012732 -0.000000010858 0.000000003941 -0.000000036936 0.000000003878 0.000000348036 0.000000002303 0.000000000029 0.000000030323 -0.000000001088 0.000000012732 0.000000002303 0.000000336507
1 1645005912.402587413788 117 118 0.103151905269 -0.007851188457 0.016492569166 -0.000133641646 -0.000389791053 -0.009425723025 0.000000107590 -0.000000031367 0.000000000521 -0.000000007632 -0.000000009840 -0.000000000231 -0.000000031367 0.000000018521 -0.000000000458 0.000000002965 0.000000003586 0.000000031051 0.000000000521 -0.000000000458 0.000000014103 -0.000000011073 -0.000000039130 -0.000000000784 -0.000000007632 0.000000002965 -0.000000011073 0.000000302435 0.000000007661 0.000000011183 -0.000000009840 0.000000003586 -0.000000039130 0.000000007661 0.000000360891 0.000000001946 -0.000000000231 0.000000031051 -0.000000000784 0.000000011183 0.000000001946 0.000000341818
1 1645005912.504237890244 118 119 0.102245964023 -0.001922173039 0.001980514565 0.000009290998 0.000695611149 -0.010118021293 0.000000106167 -0.000000031034 0.000000000655 -0.000000007967 -0.000000009572 -0.000000000164 -0.000000031034 0.000000018711 -0.000000000505 0.000000002829 0.000000003587 0.000000033047 0.000000000655 -0.000000000505 0.000000014640 -0.000000011217 -0.000000042101 -0.000000000664 -0.000000007967 0.000000002829 -0.000000011217 0.000000298544 0.000000009862 0.000000010133 -0.000000009572 0.000000003587 -0.000000042101 0.000000009862 0.000000373640 0.000000002635 -0.000000000164 0.000000033047 -0.000000000664 0.000000010133 0.000000002635 0.000000351994
1 1645005912.604142665863 119 120 0.095380530294 -0.001108012995 -0.003083408103 0.000564478649 0.001636924297 -0.014723225025 0.000000103907 -0.000000030366 0.000000000769 -0.000000008451 -0.000000010034 -0.000000001312 -0.000000030366 0.000000018725 -0.000000000550 0.000000003003 0.000000003747 0.000000034899 0.000000000769 -0.000000000550 0.000000014355 -0.000000010463 -0.000000042267 -0.000000000698 -0.000000008451 0.000000003003 -0.000000010463 0.000000293930 0.000000008522 0.000000009804 -0.000000010034 0.000000003747 -0.000000042267 0.000000008522 0.000000375353 0.000000002938 -0.000000001312 0.000000034899 -0.000000000698 0.000000009804 0.000000002938 0.000000360377
1 1645005912.702849626541 120 121 0.093286512015 -0.005930663651 0.003868319453 0.000605384071 0.000126378577 -0.016197853324 0.000000099769 -0.000000029448 0.000000000826 -0.000000008585 -0.000000009969 -0.000000002554 -0.000000029448 0.000000018277 -0.000000000493 0.000000003133 0.000000003425 0.000000035023 0.000000000826 -0.000000000493 0.000000013658 -0.000000009979 -0.000000041204 -0.000000000428 -0.000000008585 0.000000003133 -0.000000009979 0.000000289021 0.000000008461 0.000000009199 -0.000000009969 0.000000003425 -0.000000041204 0.000000008461 0.000000371803 0.000000000932 -0.000000002554 0.000000035023 -0.000000000428 0.000000009199 0.000000000932 0.000000361967
1 1645005912.802519559860 121 122 0.100987173190 0.000714157226 0.010878686943 0.000910503223 -0.000408491888 -0.016040457828 0.000000095734 -0.000000028309 0.000000000990 -0.000000008628 -0.000000009816 -0.000000001987 -0.000000028309 0.000000017686 -0.000000000553 0.000000003081 0.000000003373 0.000000034512 0.000000000990 -0.000000000553 0.000000013034 -0.000000009194 -0.000000040584 -0.000000000408 -0.000000008628 0.000000003081 -0.000000009194 0.000000282506 0.000000007004 0.000000008055 -0.000000009816 0.000000003373 -0.000000040584 0.000000007004 0.000000370148 0.000000000056 -0.000000001987 0.000000034512 -0.000000000408 0.000000008055 0.000000000056 0.000000360170
1 1645005912.902120351791 122 123 0.105744414544 0.002989777360 0.008055909981 0.000522713016 -0.000712091928 -0.015860847116 0.000000092000 -0.000000026892 0.000000001096 -0.000000007960 -0.000000008934 -0.000000001199 -0.000000026892 0.000000017033 -0.000000000612 0.000000002687 0.000000003198 0.000000034106 0.000000001096 -0.000000000612 0.000000012601 -0.000000008599 -0.000000041253 -0.000000000537 -0.000000007960 0.000000002687 -0.000000008599 0.000000278439 0.000000006637 0.000000006944 -0.000000008934 0.000000003198 -0.000000041253 0.000000006637 0.000000377733 0.000000000207 -0.000000001199 0.000000034106 -0.000000000537 0.000000006944 0.000000000207 0.000000356190
1 1645005913.005158424377 123 124 0.108371709292 0.001027107202 0.003562412218 0.000635368012 0.002115674008 -0.017401447292 0.000000088452 -0.000000025564 0.000000000949 -0.000000006689 -0.000000006146 -0.000000000882 -0.000000025564 0.000000016427 -0.000000000555 0.000000002086 0.000000002362 0.000000033970 0.000000000949 -0.000000000555 0.000000012646 -0.000000008906 -0.000000043562 -0.000000000533 -0.000000006689 0.000000002086 -0.000000008906 0.000000277482 0.000000008766 0.000000005869 -0.000000006146 0.000000002362 -0.000000043562 0.000000008766 0.000000394236 -0.000000000337 -0.000000000882 0.000000033970 -0.000000000533 0.000000005869 -0.000000000337 0.000000354101
1 1645005913.103281021118 124 125 0.102758309871 -0.002935497442 -0.002834141245 0.000992244923 0.000603097177 -0.010505902649 0.000000084679 -0.000000024480 0.000000000694 -0.000000006203 -0.000000004434 -0.000000000786 -0.000000024480 0.000000015926 -0.000000000460 0.000000001960 0.000000001912 0.000000034312 0.000000000694 -0.000000000460 0.000000012635 -0.000000008930 -0.000000045449 -0.000000000614 -0.000000006203 0.000000001960 -0.000000008930 0.000000278203 0.000000010319 0.000000005903 -0.000000004434 0.000000001912 -0.000000045449 0.000000010319 0.000000412749 0.000000000610 -0.000000000786 0.000000034312 -0.000000000614 0.000000005903 0.000000000610 0.000000357488
1 1645005913.203158855438 125 126 0.106915139328 -0.005362769587 0.005671755512 0.000982730101 0.001719758282 -0.007181197185 0.000000079753 -0.000000022565 0.000000000775 -0.000000006067 -0.000000004255 -0.000000000741 -0.000000022565 0.000000015327 -0.000000000457 0.000000001948 0.000000001834 0.000000036065 0.000000000775 -0.000000000457 0.000000012307 -0.000000008166 -0.000000046153 -0.000000000719 -0.000000006067 0.000000001948 -0.000000008166 0.000000275329 0.000000010591 0.000000006327 -0.000000004255 0.000000001834 -0.000000046153 0.000000010591 0.000000425655 0.000000001717 -0.000000000741 0.000000036065 -0.000000000719 0.000000006327 0.000000001717 0.000000367832
1 1645005913.305967569351 126 127 0.114387020145 -0.004210180518 0.015020722820 -0.000184024553 -0.001805515269 0.000786641202 0.000000073336 -0.000000020355 0.000000000681 -0.000000005855 -0.000000003624 -0.000000000918 -0.000000020355 0.000000014826 -0.000000000406 0.000000001943 0.000000001386 0.000000038378 0.000000000681 -0.000000000406 0.000000012043 -0.000000007381 -0.000000046646 -0.000000000532 -0.000000005855 0.000000001943 -0.000000007381 0.000000270632 0.000000009755 0.000000006505 -0.000000003624 0.000000001386 -0.000000046646 0.000000009755 0.000000431777 0.000000000725 -0.000000000918 0.000000038378 -0.000000000532 0.000000006505 0.000000000725 0.000000380235
1 1645005913.402428627014 127 128 0.113069559789 -0.000704930784 0.014288948611 -0.000164686473 0.000124000769 0.002486017239 0.000000067177 -0.000000018092 0.000000000770 -0.000000005230 -0.000000002337 -0.000000001447 -0.000000018092 0.000000014158 -0.000000000419 0.000000001621 0.000000000810 0.000000040400 0.000000000770 -0.000000000419 0.000000011887 -0.000000006732 -0.000000048215 -0.000000000372 -0.000000005230 0.000000001621 -0.000000006732 0.000000268588 0.000000010266 0.000000005978 -0.000000002337 0.000000000810 -0.000000048215 0.000000010266 0.000000444703 -0.000000000174 -0.000000001447 0.000000040400 -0.000000000372 0.000000005978 -0.000000000174 0.000000394429
1 1645005913.504843950272 128 129 0.113183456503 -0.003233214799 0.001167230142 -0.000877923266 -0.000380382302 0.004114345669 0.000000058977 -0.000000015334 0.000000000576 -0.000000004492 -0.000000000609 -0.000000003143 -0.000000015334 0.000000013437 -0.000000000338 0.000000001252 0.000000000348 0.000000043093 0.000000000576 -0.000000000338 0.000000010341 -0.000000006120 -0.000000045149 -0.000000000437 -0.000000004492 0.000000001252 -0.000000006120 0.000000262475 0.000000013212 0.000000005183 -0.000000000609 0.000000000348 -0.000000045149 0.000000013212 0.000000443863 0.000000000910 -0.000000003143 0.000000043093 -0.000000000437 0.000000005183 0.000000000910 0.000000408565
1 1645005913.601838827133 129 130 0.106480526708 -0.001386640502 -0.011980876123 0.000067753467 -0.000046515530 0.006676571512 0.000000048768 -0.000000012110 0.000000000322 -0.000000003692 -0.000000000074 -0.000000005515 -0.000000012110 0.000000012413 -0.000000000216 0.000000000924 0.000000000176 0.000000045221 0.000000000322 -0.000000000216 0.000000008513 -0.000000005462 -0.000000040526 -0.000000000491 -0.000000003692 0.000000000924 -0.000000005462 0.000000255291 0.000000015909 0.000000004681 -0.000000000074 0.000000000176 -0.000000040526 0.000000015909 0.000000437638 0.000000002802 -0.000000005515 0.000000045221 -0.000000000491 0.000000004681 0.000000002802 0.000000418105
1 1645005913.703332424164 130 131 0.108468668180 -0.000969605556 -0.002385739928 0.000092510686 0.001524255417 0.001870569352 0.000000039925 -0.000000009382 0.000000000084 -0.000000002806 0.000000000135 -0.000000007330 -0.000000009382 0.000000011452 -0.000000000090 0.000000000564 -0.000000000086 0.000000046436 0.000000000084 -0.000000000090 0.000000007595 -0.000000004539 -0.000000038331 -0.000000000262 -0.000000002806 0.000000000564 -0.000000004539 0.000000246399 0.000000015747 0.000000003991 0.000000000135 -0.000000000086 -0.000000038331 0.000000015747 0.000000435816 0.000000003091 -0.000000007330 0.000000046436 -0.000000000262 0.000000003991 0.000000003091 0.000000424368
1 1645005913.802533626556 131 132 0.104456210240 -0.004904373024 0.013892430815 -0.000421937248 -0.001963824939 0.001738357414 0.000000033848 -0.000000007783 0.000000000093 -0.000000002527 0.000000000077 -0.000000008221 -0.000000007783 0.000000010852 -0.000000000071 0.000000000554 -0.000000000234 0.000000046899 0.000000000093 -0.000000000071 0.000000007219 -0.000000004204 -0.000000037396 -0.000000000113 -0.000000002527 0.000000000554 -0.000000004204 0.000000236147 0.000000019026 0.000000004261 0.000000000077 -0.000000000234 -0.000000037396 0.000000019026 0.000000432716 0.000000002615 -0.000000008221 0.000000046899 -0.000000000113 0.000000004261 0.000000002615 0.000000427562
1 1645005913.902822494507 132 133 0.105252268899 -0.005892393425 0.020783173943 -0.000244242694 -0.000148631160 -0.003208653521 0.000000028686 -0.000000006554 0.000000000243 -0.000000002229 -0.000000000013 -0.000000008804 -0.000000006554 0.000000010280 -0.000000000154 0.000000000313 -0.000000000124 0.000000046583 0.000000000243 -0.000000000154 0.000000007239 -0.000000004543 -0.000000037125 -0.000000000235 -0.000000002229 0.000000000313 -0.000000004543 0.000000229949 0.000000026748 0.000000003642 -0.000000000013 -0.000000000124 -0.000000037125 0.000000026748 0.000000428474 0.000000002848 -0.000000008804 0.000000046583 -0.000000000235 0.000000003642 0.000000002848 0.000000427422
1 1645005914.004937648773 133 134 0.109519905526 -0.010991758570 0.010722232350 0.000333129949 -0.000174623972 -0.002993424422 0.000000024093 -0.000000005515 0.000000000350 -0.000000001671 -0.000000000218 -0.000000008888 -0.000000005515 0.000000009905 -0.000000000203 0.000000000110 -0.000000000103 0.000000046779 0.000000000350 -0.000000000203 0.000000006866 -0.000000005080 -0.000000035414 -0.000000000417 -0.000000001671 0.000000000110 -0.000000005080 0.000000225283 0.000000035405 0.000000003187 -0.000000000218 -0.000000000103 -0.000000035414 0.000000035405 0.000000419305 0.000000003321 -0.000000008888 0.000000046779 -0.000000000417 0.000000003187 0.000000003321 0.000000429289
1 1645005914.104001283646 134 135 0.110141859157 -0.015877687071 -0.003919154582 0.001363151861 -0.000134739250 0.000119752527 0.000000020081 -0.000000004548 0.000000000494 -0.000000000564 -0.000000000812 -0.000000008086 -0.000000004548 0.000000009513 -0.000000000212 -0.000000000183 -0.000000000079 0.000000046971 0.000000000494 -0.000000000212 0.000000006414 -0.000000005263 -0.000000033246 -0.000000000660 -0.000000000564 -0.000000000183 -0.000000005263 0.000000222499 0.000000041553 0.000000002348 -0.000000000812 -0.000000000079 -0.000000033246 0.000000041553 0.000000404705 0.000000004584 -0.000000008086 0.000000046971 -0.000000000660 0.000000002348 0.000000004584 0.000000434501
1 1645005914.205260753632 135 136 0.112731195464 -0.016646210746 0.002628785458 0.000209927570 0.000614736609 0.002584125950 0.000000016076 -0.000000003462 0.000000000384 0.000000000165 -0.000000000800 -0.000000006658 -0.000000003462 0.000000009059 -0.000000000133 -0.000000000606 -0.000000000101 0.000000046958 0.000000000384 -0.000000000133 0.000000005887 -0.000000005972 -0.000000030611 -0.000000000603 0.000000000165 -0.000000000606 -0.000000005972 0.000000219212 0.000000046082 -0.000000000162 -0.000000000800 -0.000000000101 -0.000000030611 0.000000046082 0.000000380793 0.000000005566 -0.000000006658 0.000000046958 -0.000000000603 -0.000000000162 0.000000005566 0.000000437954
1 1645005914.304635286331 136 137 0.110109539783 -0.017998414611 0.012810929145 -0.000245315305 0.000890363266 0.003698770995 0.000000013318 -0.000000002600 0.000000000326 0.000000000152 -0.000000000681 -0.000000005410 -0.000000002600 0.000000008819 -0.000000000077 -0.000000000756 -0.000000000134 0.000000047689 0.000000000326 -0.000000000077 0.000000005647 -0.000000007102 -0.000000029962 -0.000000000471 0.000000000152 -0.000000000756 -0.000000007102 0.000000215325 0.000000049153 -0.000000002005 -0.000000000681 -0.000000000134 -0.000000029962 0.000000049153 0.000000369953 0.000000005821 -0.000000005410 0.000000047689 -0.000000000471 -0.000000002005 0.000000005821 0.000000443851
1 1645005914.402868032455 137 138 0.111083721384 -0.015259298484 0.014103141251 -0.000356166718 -0.001745079695 0.006088809825 0.000000011488 -0.000000002155 0.000000000288 0.000000000087 -0.000000000634 -0.000000004446 -0.000000002155 0.000000008887 -0.000000000073 -0.000000000761 -0.000000000109 0.000000049551 0.000000000288 -0.000000000073 0.000000005632 -0.000000007521 -0.000000030497 -0.000000000488 0.000000000087 -0.000000000761 -0.000000007521 0.000000212402 0.000000048080 -0.000000002791 -0.000000000634 -0.000000000109 -0.000000030497 0.000000048080 0.000000366926 0.000000005864 -0.000000004446 0.000000049551 -0.000000000488 -0.000000002791 0.000000005864 0.000000458229
1 1645005914.502975702286 138 139 0.113945330153 -0.011103091303 0.010129852948 -0.000443467816 0.000074664741 0.003419050409 0.000000010923 -0.000000002096 0.000000000221 0.000000000018 -0.000000000160 -0.000000003828 -0.000000002096 0.000000008891 -0.000000000128 -0.000000000737 0.000000000133 0.000000050211 0.000000000221 -0.000000000128 0.000000005571 -0.000000007348 -0.000000031094 -0.000000000767 0.000000000018 -0.000000000737 -0.000000007348 0.000000208241 0.000000044856 -0.000000002913 -0.000000000160 0.000000000133 -0.000000031094 0.000000044856 0.000000367491 0.000000006917 -0.000000003828 0.000000050211 -0.000000000767 -0.000000002913 0.000000006917 0.000000464462
1 1645005914.606390953064 139 140 0.115763415418 -0.013251559319 -0.002593917162 -0.000387095926 -0.000521830202 0.004558964276 0.000000010125 -0.000000001973 0.000000000276 -0.000000000045 -0.000000000302 -0.000000002573 -0.000000001973 0.000000009055 -0.000000000160 -0.000000000851 0.000000000387 0.000000051580 0.000000000276 -0.000000000160 0.000000005080 -0.000000006897 -0.000000029536 -0.000000000890 -0.000000000045 -0.000000000851 -0.000000006897 0.000000205238 0.000000040500 -0.000000003608 -0.000000000302 0.000000000387 -0.000000029536 0.000000040500 0.000000358067 0.000000007639 -0.000000002573 0.000000051580 -0.000000000890 -0.000000003608 0.000000007639 0.000000474766
1 1645005914.705168962479 140 141 0.115950679870 -0.007920648289 -0.003581919731 -0.000183384951 -0.000202153026 0.006015096621 0.000000008937 -0.000000001667 0.000000000398 -0.000000000007 -0.000000000886 -0.000000000256 -0.000000001667 0.000000009384 -0.000000000113 -0.000000001058 0.000000000199 0.000000054119 0.000000000398 -0.000000000113 0.000000004302 -0.000000006382 -0.000000025529 -0.000000000504 -0.000000000007 -0.000000001058 -0.000000006382 0.000000203274 0.000000035133 -0.000000005395 -0.000000000886 0.000000000199 -0.000000025529 0.000000035133 0.000000328835 0.000000005386 -0.000000000256 0.000000054119 -0.000000000504 -0.000000005395 0.000000005386 0.000000494144
1 1645005914.803323030472 141 142 0.113344092687 -0.004513999756 0.003881859525 -0.000081391673 0.001628779102 0.003417929098 0.000000007946 -0.000000001342 0.000000000361 0.000000000164 -0.000000000461 0.000000001892 -0.000000001342 0.000000009145 -0.000000000107 -0.000000001104 0.000000000120 0.000000053134 0.000000000361 -0.000000000107 0.000000003877 -0.000000006332 -0.000000023177 -0.000000000447 0.000000000164 -0.000000001104 -0.000000006332 0.000000198500 0.000000032820 -0.000000006363 -0.000000000461 0.000000000120 -0.000000023177 0.000000032820 0.000000305051 0.000000004342 0.000000001892 0.000000053134 -0.000000000447 -0.000000006363 0.000000004342 0.000000489129
1 1645005914.904057264328 142 143 0.113932415653 -0.008793837129 0.010399809107 -0.000235218052 -0.001425873177 0.005105286124 0.000000007366 -0.000000001153 0.000000000221 0.000000000379 0.000000000457 0.000000003120 -0.000000001153 0.000000008786 -0.000000000141 -0.000000001114 0.000000000245 0.000000051362 0.000000000221 -0.000000000141 0.000000003860 -0.000000006582 -0.000000022852 -0.000000000726 0.000000000379 -0.000000001114 -0.000000006582 0.000000192639 0.000000031559 -0.000000006610 0.000000000457 0.000000000245 -0.000000022852 0.000000031559 0.000000290550 0.000000005271 0.000000003120 0.000000051362 -0.000000000726 -0.000000006610 0.000000005271 0.000000477340
1 1645005915.005204200745 143 144 0.114962510911 -0.012358710079 0.012393871611 -0.000551754572 -0.000248518402 0.003903856192 0.000000007433 -0.000000001098 0.000000000173 0.000000000396 0.000000000817 0.000000003762 -0.000000001098 0.000000008813 -0.000000000189 -0.000000001302 0.000000000582 0.000000051927 0.000000000173 -0.000000000189 0.000000003938 -0.000000006922 -0.000000022827 -0.000000001046 0.000000000396 -0.000000001302 -0.000000006922 0.000000188507 0.000000031757 -0.000000007689 0.000000000817 0.000000000582 -0.000000022827 0.000000031757 0.000000280243 0.000000007393 0.000000003762 0.000000051927 -0.000000001046 -0.000000007689 0.000000007393 0.000000482670
1 1645005915.104339361191 144 145 0.108638769015 -0.013109909901 0.001280725320 0.000360075563 -0.000501910880 0.003610744318 0.000000007793 -0.000000001032 0.000000000172 0.000000000318 0.000000000989 0.000000004597 -0.000000001032 0.000000008853 -0.000000000219 -0.000000001613 0.000000000892 0.000000052595 0.000000000172 -0.000000000219 0.000000004122 -0.000000007546 -0.000000023608 -0.000000001206 0.000000000318 -0.000000001613 -0.000000007546 0.000000185838 0.000000036183 -0.000000009621 0.000000000989 0.000000000892 -0.000000023608 0.000000036183 0.000000280865 0.000000009177 0.000000004597 0.000000052595 -0.000000001206 -0.000000009621 0.000000009177 0.000000490642
1 1645005915.205692529678 145 146 0.109329578702 -0.013304774566 0.000306145880 -0.000023011733 0.000369480071 0.005562445904 0.000000008439 -0.000000000978 0.000000000142 0.000000000381 0.000000001532 0.000000005640 -0.000000000978 0.000000008798 -0.000000000212 -0.000000001840 0.000000000941 0.000000053029 0.000000000142 -0.000000000212 0.000000004685 -0.000000008117 -0.000000027229 -0.000000001233 0.000000000381 -0.000000001840 -0.000000008117 0.000000182654 0.000000041408 -0.000000011114 0.000000001532 0.000000000941 -0.000000027229 0.000000041408 0.000000305374 0.000000009971 0.000000005640 0.000000053029 -0.000000001233 -0.000000011114 0.000000009971 0.000000499247
1 1645005915.303473472595 146 147 0.102615336962 -0.011723016179 0.016921966183 -0.000598724662 -0.000883449845 0.009912420693 0.000000009050 -0.000000000931 0.000000000141 0.000000000283 0.000000001632 0.000000006520 -0.000000000931 0.000000008841 -0.000000000254 -0.000000001783 0.000000001294 0.000000053986 0.000000000141 -0.000000000254 0.000000005341 -0.000000008689 -0.000000031566 -0.000000001523 0.000000000283 -0.000000001783 -0.000000008689 0.000000180735 0.000000046782 -0.000000010967 0.000000001632 0.000000001294 -0.000000031566 0.000000046782 0.000000337155 0.000000012174 0.000000006520 0.000000053986 -0.000000001523 -0.000000010967 0.000000012174 0.000000511691
1 1645005915.403654336929 147 148 0.103466727897 -0.010311299427 0.019111226280 -0.000605924141 -0.000721619272 0.014142457072 0.000000009872 -0.000000000937 0.000000000170 0.000000000104 0.000000001623 0.000000006936 -0.000000000937 0.000000009038 -0.000000000289 -0.000000001859 0.000000001533 0.000000055630 0.000000000170 -0.000000000289 0.000000005662 -0.000000008982 -0.000000033316 -0.000000001699 0.000000000104 -0.000000001859 -0.000000008982 0.000000179291 0.000000050127 -0.000000011628 0.000000001623 0.000000001533 -0.000000033316 0.000000050127 0.000000349687 0.000000013494 0.000000006936 0.000000055630 -0.000000001699 -0.000000011628 0.000000013494 0.000000526085
1 1645005915.502804994583 148 149 0.102555175381 -0.007097360397 0.012460654451 -0.001109214232 -0.001235813727 0.016759657315 0.000000010898 -0.000000001092 0.000000000180 0.000000000034 0.000000001717 0.000000006432 -0.000000001092 0.000000009110 -0.000000000312 -0.000000001774 0.000000001584 0.000000056414 0.000000000180 -0.000000000312 0.000000005889 -0.000000008997 -0.000000034270 -0.000000001851 0.000000000034 -0.000000001774 -0.000000008997 0.000000178153 0.000000052144 -0.000000011187 0.000000001717 0.000000001584 -0.000000034270 0.000000052144 0.000000356560 0.000000013909 0.000000006432 0.000000056414 -0.000000001851 -0.000000011187 0.000000013909 0.000000534819
1 1645005915.606675386429 149 150 0.110023677701 -0.005261608351 -0.001025304830 -0.000900306716 -0.001729175039 0.017897628554 0.000000011791 -0.000000001314 0.000000000193 -0.000000000199 0.000000001536 0.000000005499 -0.000000001314 0.000000009095 -0.000000000322 -0.000000001698 0.000000001815 0.000000056489 0.000000000193 -0.000000000322 0.000000006056 -0.000000009260 -0.000000035203 -0.000000001976 -0.000000000199 -0.000000001698 -0.000000009260 0.000000180591 0.000000056392 -0.000000010737 0.000000001536 0.000000001815 -0.000000035203 0.000000056392 0.000000367602 0.000000015758 0.000000005499 0.000000056489 -0.000000001976 -0.000000010737 0.000000015758 0.000000538970
1 1645005915.706191539764 150 151 0.108997921836 -0.002332104651 -0.007968681270 -0.000641057737 0.000297247851 0.013744307307 0.000000012496 -0.000000001488 0.000000000182 -0.000000000437 0.000000001129 0.000000004476 -0.000000001488 0.000000009204 -0.000000000349 -0.000000001745 0.000000002295 0.000000057627 0.000000000182 -0.000000000349 0.000000006180 -0.000000009543 -0.000000036359 -0.000000002262 -0.000000000437 -0.000000001745 -0.000000009543 0.000000185160 0.000000061278 -0.000000010792 0.000000001129 0.000000002295 -0.000000036359 0.000000061278 0.000000385670 0.000000019334 0.000000004476 0.000000057627 -0.000000002262 -0.000000010792 0.000000019334 0.000000551459
1 1645005915.803260087967 151 152 0.106514602397 -0.001770007540 -0.000096349759 -0.000752589415 0.000118381130 0.009137181083 0.000000012917 -0.000000001667 0.000000000103 -0.000000000308 0.000000001192 0.000000002903 -0.000000001667 0.000000009141 -0.000000000325 -0.000000001618 0.000000002128 0.000000057675 0.000000000103 -0.000000000325 0.000000006523 -0.000000009497 -0.000000039281 -0.000000002198 -0.000000000308 -0.000000001618 -0.000000009497 0.000000189290 0.000000063224 -0.000000009134 0.000000001192 0.000000002128 -0.000000039281 0.000000063224 0.000000417011 0.000000018931 0.000000002903 0.000000057675 -0.000000002198 -0.000000009134 0.000000018931 0.000000555912
1 1645005915.905807971954 152 153 0.109997173206 -0.004906461686 0.014498026032 -0.000862247837 0.000085107174 0.006427930427 0.000000013310 -0.000000001919 0.000000000086 -0.000000000244 0.000000001170 0.000000001331 -0.000000001919 0.000000008982 -0.000000000223 -0.000000001470 0.000000001408 0.000000056875 0.000000000086 -0.000000000223 0.000000006917 -0.000000008840 -0.000000042953 -0.000000001544 -0.000000000244 -0.000000001470 -0.000000008840 0.000000192614 0.000000059942 -0.000000007491 0.000000001170 0.000000001408 -0.000000042953 0.000000059942 0.000000453104 0.000000014715 0.000000001331 0.000000056875 -0.000000001544 -0.000000007491 0.000000014715 0.000000552905
1 1645005916.003596782684 153 154 0.105956714038 -0.007930247692 0.017803331311 -0.000325566987 -0.000424651970 0.000784271079 0.000000013892 -0.000000002247 0.000000000116 -0.000000000302 0.000000000924 -0.000000000604 -0.000000002247 0.000000009117 -0.000000000160 -0.000000001400 0.000000000976 0.000000058527 0.000000000116 -0.000000000160 0.000000007137 -0.000000007719 -0.000000045692 -0.000000001110 -0.000000000302 -0.000000001400 -0.000000007719 0.000000198654 0.000000053623 -0.000000006581 0.000000000924 0.000000000976 -0.000000045692 0.000000053623 0.000000487022 0.000000012262 -0.000000000604 0.000000058527 -0.000000001110 -0.000000006581 0.000000012262 0.000000572349
1 1645005916.099525690079 154 155 0.106714377159 -0.007351031551 0.007507753034 0.000196149893 -0.001095119436 -0.001110160472 0.000000014587 -0.000000002603 0.000000000179 -0.000000000321 0.000000000579 -0.000000002608 -0.000000002603 0.000000009454 -0.000000000165 -0.000000001330 0.000000001027 0.000000061678 0.000000000179 -0.000000000165 0.000000007113 -0.000000007149 -0.000000046636 -0.000000001161 -0.000000000321 -0.000000001330 -0.000000007149 0.000000206337 0.000000051754 -0.000000005643 0.000000000579 0.000000001027 -0.000000046636 0.000000051754 0.000000508473 0.000000013467 -0.000000002608 0.000000061678 -0.000000001161 -0.000000005643 0.000000013467 0.000000604801
1 1645005916.201919555664 155 156 0.114214238464 -0.005627023360 -0.006587383337 0.000405811911 0.001118760483 -0.003412120900 0.000000016892 -0.000000003571 0.000000000307 -0.000000000279 0.000000000065 -0.000000005502 -0.000000003571 0.000000010098 -0.000000000203 -0.000000001503 0.000000001164 0.000000066149 0.000000000307 -0.000000000203 0.000000007107 -0.000000007131 -0.000000047845 -0.000000001264 -0.000000000279 -0.000000001503 -0.000000007131 0.000000211613 0.000000054896 -0.000000007056 0.000000000065 0.000000001164 -0.000000047845 0.000000054896 0.000000531070 0.000000014713 -0.000000005502 0.000000066149 -0.000000001264 -0.000000007056 0.000000014713 0.000000647179
1 1645005916.305266141891 156 157 0.111308342148 -0.009772812253 -0.005809218470 -0.000158830582 -0.000215490923 0.000333338897 0.000000024402 -0.000000006436 0.000000000493 -0.000000000261 -0.000000000377 -0.000000011240 -0.000000006436 0.000000011477 -0.000000000292 -0.000000001398 0.000000001461 0.000000072198 0.000000000493 -0.000000000292 0.000000007568 -0.000000007942 -0.000000052886 -0.000000001576 -0.000000000261 -0.000000001398 -0.000000007942 0.000000218297 0.000000065152 -0.000000006844 -0.000000000377 0.000000001461 -0.000000052886 0.000000065152 0.000000590097 0.000000017471 -0.000000011240 0.000000072198 -0.000000001576 -0.000000006844 0.000000017471 0.000000700700
1 1645005916.402242660522 157 158 0.098861612320 -0.010532723695 0.012346443386 0.000302995827 0.000414997200 0.001602970315 0.000000037109 -0.000000010805 0.000000000821 -0.000000000804 -0.000000001279 -0.000000016111 -0.000000010805 0.000000012886 -0.000000000505 -0.000000000852 0.000000002599 0.000000074435 0.000000000821 -0.000000000505 0.000000008686 -0.000000010526 -0.000000062013 -0.000000002548 -0.000000000804 -0.000000000852 -0.000000010526 0.000000231195 0.000000086125 -0.000000003957 -0.000000001279 0.000000002599 -0.000000062013 0.000000086125 0.000000677017 0.000000025691 -0.000000016111 0.000000074435 -0.000000002548 -0.000000003957 0.000000025691 0.000000727276
1 1645005916.503258705139 158 159 0.102502168086 -0.009093252456 0.014363853030 -0.000283008408 -0.000358165826 0.006681060732 0.000000051140 -0.000000015225 0.000000001216 -0.000000002070 -0.000000003404 -0.000000014930 -0.000000015225 0.000000013941 -0.000000000656 -0.000000000227 0.000000003539 0.000000072261 0.000000001216 -0.000000000656 0.000000009605 -0.000000014137 -0.000000068917 -0.000000002838 -0.000000002070 -0.000000000227 -0.000000014137 0.000000246310 0.000000111562 -0.000000001761 -0.000000003404 0.000000003539 -0.000000068917 0.000000111562 0.000000735475 0.000000029040 -0.000000014930 0.000000072261 -0.000000002838 -0.000000001761 0.000000029040 0.000000724038
1 1645005916.603333711624 159 160 0.110584118510 -0.006969165435 0.006755957308 -0.000547151088 -0.002765490415 0.010723492151 0.000000064963 -0.000000019500 0.000000001613 -0.000000003508 -0.000000006554 -0.000000011097 -0.000000019500 0.000000015237 -0.000000000783 0.000000000383 0.000000004607 0.000000071746 0.000000001613 -0.000000000783 0.000000010020 -0.000000017014 -0.000000072512 -0.000000002905 -0.000000003508 0.000000000383 -0.000000017014 0.000000260411 0.000000131565 -0.000000000900 -0.000000006554 0.000000004607 -0.000000072512 0.000000131565 0.000000769488 0.000000030950 -0.000000011097 0.000000071746 -0.000000002905 -0.000000000900 0.000000030950 0.000000738042
1 1645005916.703304529190 160 161 0.115867178906 -0.001405365939 -0.006941536777 -0.000217074623 0.000672686205 0.003645953857 0.000000078026 -0.000000023457 0.000000002054 -0.000000004985 -0.000000009324 -0.000000008604 -0.000000023457 0.000000016517 -0.000000000950 0.000000000749 0.000000005758 0.000000073233 0.000000002054 -0.000000000950 0.000000010587 -0.000000019544 -0.000000077702 -0.000000003100 -0.000000004985 0.000000000749 -0.000000019544 0.000000277224 0.000000150069 -0.000000002565 -0.000000009324 0.000000005758 -0.000000077702 0.000000150069 0.000000821178 0.000000033645 -0.000000008604 0.000000073233 -0.000000003100 -0.000000002565 0.000000033645 0.000000772113
1 1645005916.804172515869 161 162 0.117302909972 0.000295419838 -0.016017415108 -0.000106164346 0.000901201232 -0.001277739925 0.000000088382 -0.000000026988 0.000000002179 -0.000000006324 -0.000000011465 -0.000000008722 -0.000000026988 0.000000017842 -0.000000000989 0.000000001031 0.000000006457 0.000000076083 0.000000002179 -0.000000000989 0.000000010901 -0.000000021540 -0.000000081165 -0.000000002920 -0.000000006324 0.000000001031 -0.000000021540 0.000000295157 0.000000164011 -0.000000004981 -0.000000011465 0.000000006457 -0.000000081165 0.000000164011 0.000000859137 0.000000032540 -0.000000008722 0.000000076083 -0.000000002920 -0.000000004981 0.000000032540 0.000000810795
1 1645005916.907030344009 162 163 0.117113502778 -0.001332837421 -0.000358495722 0.000214441813 0.001777967441 -0.003905204155 0.000000096512 -0.000000029855 0.000000002263 -0.000000007359 -0.000000013389 -0.000000009255 -0.000000029855 0.000000018930 -0.000000001023 0.000000001324 0.000000007208 0.000000077916 0.000000002263 -0.000000001023 0.000000011112 -0.000000023344 -0.000000084006 -0.000000002951 -0.000000007359 0.000000001324 -0.000000023344 0.000000313829 0.000000177054 -0.000000006430 -0.000000013389 0.000000007208 -0.000000084006 0.000000177054 0.000000893655 0.000000033005 -0.000000009255 0.000000077916 -0.000000002951 -0.000000006430 0.000000033005 0.000000834816
1 1645005917.005794048309 163 164 0.112162385129 -0.003856647165 0.014586076375 -0.000117380248 -0.001608061862 -0.001170630734 0.000000103324 -0.000000032168 0.000000002342 -0.000000008716 -0.000000016076 -0.000000006607 -0.000000032168 0.000000019618 -0.000000000993 0.000000001842 0.000000007717 0.000000077445 0.000000002342 -0.000000000993 0.000000011325 -0.000000025511 -0.000000087156 -0.000000002530 -0.000000008716 0.000000001842 -0.000000025511 0.000000334949 0.000000195314 -0.000000006232 -0.000000016076 0.000000007717 -0.000000087156 0.000000195314 0.000000934604 0.000000029769 -0.000000006607 0.000000077445 -0.000000002530 -0.000000006232 0.000000029769 0.000000849058
1 1645005917.104857444763 164 165 0.111741027745 -0.003567459006 0.022616849137 0.000236524934 0.000178204487 -0.002832779338 0.000000108247 -0.000000033657 0.000000002538 -0.000000009343 -0.000000017174 -0.000000003909 -0.000000033657 0.000000020124 -0.000000000963 0.000000001696 0.000000007303 0.000000078131 0.000000002538 -0.000000000963 0.000000011442 -0.000000026989 -0.000000089241 -0.000000001718 -0.000000009343 0.000000001696 -0.000000026989 0.000000352166 0.000000208528 -0.000000009427 -0.000000017174 0.000000007303 -0.000000089241 0.000000208528 0.000000963343 0.000000021895 -0.000000003909 0.000000078131 -0.000000001718 -0.000000009427 0.000000021895 0.000000871104
1 1645005917.207338571548 165 166 0.113903906676 -0.004144900189 0.009725110224 0.000086239947 -0.000462914316 -0.004704134184 0.000000110910 -0.000000034448 0.000000002432 -0.000000009395 -0.000000015619 -0.000000003444 -0.000000034448 0.000000020431 -0.000000000941 0.000000001671 0.000000006954 0.000000079230 0.000000002432 -0.000000000941 0.000000011953 -0.000000028963 -0.000000094423 -0.000000001837 -0.000000009395 0.000000001671 -0.000000028963 0.000000367870 0.000000226360 -0.000000010076 -0.000000015619 0.000000006954 -0.000000094423 0.000000226360 0.000001016065 0.000000022453 -0.000000003444 0.000000079230 -0.000000001837 -0.000000010076 0.000000022453 0.000000886686
1 1645005917.305225610733 166 167 0.106350650780 -0.008006138847 -0.004136760500 0.000217735543 0.000793264977 -0.005450629162 0.000000112457 -0.000000034975 0.000000002459 -0.000000009900 -0.000000014669 -0.000000004888 -0.000000034975 0.000000020691 -0.000000001068 0.000000002175 0.000000007608 0.000000081541 0.000000002459 -0.000000001068 0.000000012417 -0.000000030753 -0.000000100017 -0.000000002845 -0.000000009900 0.000000002175 -0.000000030753 0.000000385554 0.000000244403 -0.000000007583 -0.000000014669 0.000000007608 -0.000000100017 0.000000244403 0.000001081297 0.000000030238 -0.000000004888 0.000000081541 -0.000000002845 -0.000000007583 0.000000030238 0.000000913045
1 1645005917.404884338379 167 168 0.098821449951 -0.014936367447 0.003157378141 0.000960141444 0.000553150047 -0.007218868305 0.000000114058 -0.000000035351 0.000000002309 -0.000000010234 -0.000000013984 -0.000000006367 -0.000000035351 0.000000020849 -0.000000000967 0.000000002122 0.000000006908 0.000000083703 0.000000002309 -0.000000000967 0.000000012562 -0.000000031523 -0.000000102262 -0.000000002356 -0.000000010234 0.000000002122 -0.000000031523 0.000000397207 0.000000252081 -0.000000009475 -0.000000013984 0.000000006908 -0.000000102262 0.000000252081 0.000001111583 0.000000025955 -0.000000006367 0.000000083703 -0.000000002356 -0.000000009475 0.000000025955 0.000000936514
1 1645005917.504653453827 168 169 0.094186437816 -0.015526879719 0.019416391189 0.000802075926 0.000404113705 -0.007307554869 0.000000114991 -0.000000035620 0.000000002454 -0.000000011103 -0.000000015611 -0.000000006733 -0.000000035620 0.000000020791 -0.000000000965 0.000000002157 0.000000007149 0.000000083370 0.000000002454 -0.000000000965 0.000000012360 -0.000000031514 -0.000000100745 -0.000000002049 -0.000000011103 0.000000002157 -0.000000031514 0.000000399690 0.000000251423 -0.000000012007 -0.000000015611 0.000000007149 -0.000000100745 0.000000251423 0.000001101197 0.000000025036 -0.000000006733 0.000000083370 -0.000000002049 -0.000000012007 0.000000025036 0.000000937610
1 1645005917.605074167252 169 170 0.100592303433 -0.011605821285 0.019967308871 0.000000189307 -0.002053301407 -0.000766444936 0.000000116137 -0.000000036111 0.000000002855 -0.000000013056 -0.000000020038 -0.000000003199 -0.000000036111 0.000000021073 -0.000000001193 0.000000003208 0.000000009429 0.000000083889 0.000000002855 -0.000000001193 0.000000011993 -0.000000030942 -0.000000098311 -0.000000002956 -0.000000013056 0.000000003208 -0.000000030942 0.000000398169 0.000000245893 -0.000000009126 -0.000000020038 0.000000009429 -0.000000098311 0.000000245893 0.000001086821 0.000000034002 -0.000000003199 0.000000083889 -0.000000002956 -0.000000009126 0.000000034002 0.000000959796
1 1645005917.706811666489 170 171 0.107970526617 -0.008036241272 0.010229935133 -0.000213881979 -0.001054988989 0.000031095351 0.000000118125 -0.000000036312 0.000000003318 -0.000000014955 -0.000000023459 0.000000001488 -0.000000036312 0.000000021160 -0.000000001345 0.000000003695 0.000000010662 0.000000084616 0.000000003318 -0.000000001345 0.000000011958 -0.000000030573 -0.000000098713 -0.000000003110 -0.000000014955 0.000000003695 -0.000000030573 0.000000397435 0.000000242189 -0.000000009785 -0.000000023459 0.000000010662 -0.000000098713 0.000000242189 0.000001096053 0.000000036591 0.000000001488 0.000000084616 -0.000000003110 -0.000000009785 0.000000036591 0.000000988045
1 1645005917.804672718048 171 172 0.105731298009 -0.005919607258 -0.005833779357 -0.000045148203 0.001018874621 -0.000751755422 0.000000121189 -0.000000036830 0.000000003468 -0.000000015684 -0.000000023681 0.000000004698 -0.000000036830 0.000000021195 -0.000000001433 0.000000003788 0.000000011194 0.000000084668 0.000000003468 -0.000000001433 0.000000012248 -0.000000031571 -0.000000101807 -0.000000003557 -0.000000015684 0.000000003788 -0.000000031571 0.000000404763 0.000000251181 -0.000000010529 -0.000000023681 0.000000011194 -0.000000101807 0.000000251181 0.000001129929 0.000000041603 0.000000004698 0.000000084668 -0.000000003557 -0.000000010529 0.000000041603 0.000001008043
1 1645005917.904610872269 172 173 0.103855440730 -0.008847785631 -0.010570833324 0.000073912894 0.000524173989 -0.000244240168 0.000000124632 -0.000000037627 0.000000003556 -0.000000016897 -0.000000025958 0.000000007236 -0.000000037627 0.000000021390 -0.000000001541 0.000000004351 0.000000012531 0.000000085042 0.000000003556 -0.000000001541 0.000000012424 -0.000000032522 -0.000000104071 -0.000000004167 -0.000000016897 0.000000004351 -0.000000032522 0.000000412569 0.000000261172 -0.000000009421 -0.000000025958 0.000000012531 -0.000000104071 0.000000261172 0.000001156601 0.000000046360 0.000000007236 0.000000085042 -0.000000004167 -0.000000009421 0.000000046360 0.000001026787
1 1645005918.005419969559 173 174 0.100505315538 -0.010103880265 0.006164128240 0.000413598049 0.001946711804 -0.004386893254 0.000000127051 -0.000000038318 0.000000003856 -0.000000018597 -0.000000029537 0.000000006021 -0.000000038318 0.000000021496 -0.000000001582 0.000000004785 0.000000013137 0.000000085273 0.000000003856 -0.000000001582 0.000000012143 -0.000000032181 -0.000000103060 -0.000000003657 -0.000000018597 0.000000004785 -0.000000032181 0.000000415569 0.000000261713 -0.000000011012 -0.000000029537 0.000000013137 -0.000000103060 0.000000261713 0.000001158654 0.000000041063 0.000000006021 0.000000085273 -0.000000003657 -0.000000011012 0.000000041063 0.000001031052
1 1645005918.106912612915 174 175 0.104243823486 -0.015398520529 0.017609900953 0.000251278556 -0.001321653647 -0.002699090350 0.000000129650 -0.000000039225 0.000000003859 -0.000000019970 -0.000000031925 0.000000007468 -0.000000039225 0.000000021632 -0.000000001548 0.000000005349 0.000000013611 0.000000083970 0.000000003859 -0.000000001548 0.000000012009 -0.000000032668 -0.000000103462 -0.000000003323 -0.000000019970 0.000000005349 -0.000000032668 0.000000423439 0.000000269388 -0.000000010299 -0.000000031925 0.000000013611 -0.000000103462 0.000000269388 0.000001178424 0.000000038174 0.000000007468 0.000000083970 -0.000000003323 -0.000000010299 0.000000038174 0.000001031326
1 1645005918.205326795578 175 176 0.105018001061 -0.016620276316 0.016583575074 0.000471605107 -0.001302930249 -0.006284028454 0.000000132952 -0.000000039810 0.000000004127 -0.000000022074 -0.000000034193 0.000000010673 -0.000000039810 0.000000021502 -0.000000001615 0.000000005775 0.000000014293 0.000000082301 0.000000004127 -0.000000001615 0.000000012306 -0.000000034229 -0.000000107844 -0.000000003231 -0.000000022074 0.000000005775 -0.000000034229 0.000000435942 0.000000288374 -0.000000012012 -0.000000034193 0.000000014293 -0.000000107844 0.000000288374 0.000001238900 0.000000038299 0.000000010673 0.000000082301 -0.000000003231 -0.000000012012 0.000000038299 0.000001036436
1 1645005918.304870605469 176 177 0.104912611278 -0.018585854244 0.001515240424 0.000485324157 0.001946316449 -0.010479077493 0.000000136347 -0.000000040849 0.000000004059 -0.000000022351 -0.000000030809 0.000000009278 -0.000000040849 0.000000021957 -0.000000001504 0.000000004860 0.000000012760 0.000000085030 0.000000004059 -0.000000001504 0.000000012626 -0.000000035744 -0.000000112385 -0.000000002494 -0.000000022351 0.000000004860 -0.000000035744 0.000000448023 0.000000308545 -0.000000021449 -0.000000030809 0.000000012760 -0.000000112385 0.000000308545 0.000001300935 0.000000034558 0.000000009278 0.000000085030 -0.000000002494 -0.000000021449 0.000000034558 0.000001068746
1 1645005918.405547380447 177 178 0.104792166995 -0.023226516703 -0.007056980576 0.000267709258 0.000474336908 -0.007207889728 0.000000138617 -0.000000041762 0.000000003857 -0.000000022163 -0.000000029275 0.000000010678 -0.000000041762 0.000000022377 -0.000000001391 0.000000004532 0.000000011951 0.000000086241 0.000000003857 -0.000000001391 0.000000012725 -0.000000037131 -0.000000114801 -0.000000001984 -0.000000022163 0.000000004532 -0.000000037131 0.000000457244 0.000000327019 -0.000000025297 -0.000000029275 0.000000011951 -0.000000114801 0.000000327019 0.000001339780 0.000000031499 0.000000010678 0.000000086241 -0.000000001984 -0.000000025297 0.000000031499 0.000001092396
1 1645005918.505361080170 178 179 0.102238431531 -0.023908947381 0.006589654277 0.000655843708 0.000453140817 -0.001821223501 0.000000140246 -0.000000042197 0.000000004132 -0.000000023003 -0.000000031421 0.000000015985 -0.000000042197 0.000000022353 -0.000000001641 0.000000005629 0.000000014034 0.000000084232 0.000000004132 -0.000000001641 0.000000012973 -0.000000038726 -0.000000118299 -0.000000003433 -0.000000023003 0.000000005629 -0.000000038726 0.000000462890 0.000000346326 -0.000000018649 -0.000000031421 0.000000014034 -0.000000118299 0.000000346326 0.000001385059 0.000000044386 0.000000015985 0.000000084232 -0.000000003433 -0.000000018649 0.000000044386 0.000001097883
1 1645005918.605193614960 179 180 0.107683008826 -0.022764995092 0.017514407486 -0.000368667307 -0.001653800364 0.007067164909 0.000000141481 -0.000000042435 0.000000004360 -0.000000024495 -0.000000034469 0.000000024183 -0.000000042435 0.000000022389 -0.000000001837 0.000000006705 0.000000016054 0.000000082514 0.000000004360 -0.000000001837 0.000000013111 -0.000000040013 -0.000000120651 -0.000000004389 -0.000000024495 0.000000006705 -0.000000040013 0.000000466496 0.000000363613 -0.000000014517 -0.000000034469 0.000000016054 -0.000000120651 0.000000363613 0.000001419210 0.000000053423 0.000000024183 0.000000082514 -0.000000004389 -0.000000014517 0.000000053423 0.000001113406
1 1645005918.704743146896 180 181 0.111753784251 -0.017339671469 0.014827032018 -0.000550269913 -0.000679567626 0.009481384874 0.000000142307 -0.000000041952 0.000000004661 -0.000000025741 -0.000000036641 0.000000033488 -0.000000041952 0.000000022051 -0.000000001803 0.000000006655 0.000000015521 0.000000081558 0.000000004661 -0.000000001803 0.000000012906 -0.000000039951 -0.000000119907 -0.000000003114 -0.000000025741 0.000000006655 -0.000000039951 0.000000465724 0.000000367203 -0.000000019105 -0.000000036641 0.000000015521 -0.000000119907 0.000000367203 0.000001423450 0.000000042472 0.000000033488 0.000000081558 -0.000000003114 -0.000000019105 0.000000042472 0.000001147641
1 1645005918.806400060654 181 182 0.113872331121 -0.014001660104 -0.000550563706 -0.000917522817 -0.000618426666 0.010240698071 0.000000143709 -0.000000041830 0.000000004468 -0.000000025299 -0.000000034644 0.000000039964 -0.000000041830 0.000000022150 -0.000000001671 0.000000005770 0.000000014366 0.000000084352 0.000000004468 -0.000000001671 0.000000012897 -0.000000040281 -0.000000121040 -0.000000002385 -0.000000025299 0.000000005770 -0.000000040281 0.000000469448 0.000000373553 -0.000000026765 -0.000000034644 0.000000014366 -0.000000121040 0.000000373553 0.000001447927 0.000000037412 0.000000039964 0.000000084352 -0.000000002385 -0.000000026765 0.000000037412 0.000001214810
1 1645005918.905241250992 182 183 0.108627949209 -0.010341888855 -0.014273654629 -0.000517403403 0.001641954984 0.006780294180 0.000000145842 -0.000000041995 0.000000004312 -0.000000023934 -0.000000031977 0.000000043324 -0.000000041995 0.000000022211 -0.000000001575 0.000000004796 0.000000013177 0.000000086375 0.000000004312 -0.000000001575 0.000000013350 -0.000000041889 -0.000000127167 -0.000000001918 -0.000000023934 0.000000004796 -0.000000041889 0.000000478700 0.000000395388 -0.000000032141 -0.000000031977 0.000000013177 -0.000000127167 0.000000395388 0.000001528684 0.000000033922 0.000000043324 0.000000086375 -0.000000001918 -0.000000032141 0.000000033922 0.000001262581
1 1645005919.006230831146 183 184 0.111090918097 -0.011375888755 -0.011195486808 -0.000492323663 -0.000760811117 0.007360552053 0.000000147026 -0.000000042366 0.000000004035 -0.000000022992 -0.000000032173 0.000000045126 -0.000000042366 0.000000022179 -0.000000001481 0.000000004851 0.000000013121 0.000000085324 0.000000004035 -0.000000001481 0.000000013612 -0.000000043406 -0.000000131031 -0.000000001914 -0.000000022992 0.000000004851 -0.000000043406 0.000000485597 0.000000416394 -0.000000028771 -0.000000032173 0.000000013121 -0.000000131031 0.000000416394 0.000001580709 0.000000032816 0.000000045126 0.000000085324 -0.000000001914 -0.000000028771 0.000000032816 0.000001269063
1 1645005919.103321552277 184 185 0.106064541975 -0.013212314792 0.007933789346 -0.000093460789 -0.000240302902 0.002770392774 0.000000147409 -0.000000042125 0.000000004336 -0.000000023061 -0.000000034649 0.000000046124 -0.000000042125 0.000000021985 -0.000000001589 0.000000004937 0.000000014153 0.000000085861 0.000000004336 -0.000000001589 0.000000013574 -0.000000043836 -0.000000132647 -0.000000002199 -0.000000023061 0.000000004937 -0.000000043836 0.000000489170 0.000000427692 -0.000000028221 -0.000000034649 0.000000014153 -0.000000132647 0.000000427692 0.000001615546 0.000000036449 0.000000046124 0.000000085861 -0.000000002199 -0.000000028221 0.000000036449 0.000001285384
1 1645005919.204598665237 185 186 0.109583227748 -0.018751742246 0.015930061005 -0.000207377330 -0.000969420842 -0.001611917201 0.000000147737 -0.000000042240 0.000000004321 -0.000000022740 -0.000000033885 0.000000043695 -0.000000042240 0.000000022018 -0.000000001592 0.000000004837 0.000000014065 0.000000087852 0.000000004321 -0.000000001592 0.000000013380 -0.000000043618 -0.000000132567 -0.000000002397 -0.000000022740 0.000000004837 -0.000000043618 0.000000492109 0.000000430403 -0.000000028113 -0.000000033885 0.000000014065 -0.000000132567 0.000000430403 0.000001632040 0.000000039106 0.000000043695 0.000000087852 -0.000000002397 -0.000000028113 0.000000039106 0.000001310151
1 1645005919.304369688034 186 187 0.105935726310 -0.021962018124 0.012691866608 0.000577732489 0.000249586532 -0.006153890357 0.000000149020 -0.000000042831 0.000000003937 -0.000000021091 -0.000000027669 0.000000039205 -0.000000042831 0.000000022289 -0.000000001497 0.000000004450 0.000000012366 0.000000090284 0.000000003937 -0.000000001497 0.000000013677 -0.000000045058 -0.000000137345 -0.000000002732 -0.000000021091 0.000000004450 -0.000000045058 0.000000500943 0.000000450114 -0.000000025733 -0.000000027669 0.000000012366 -0.000000137345 0.000000450114 0.000001702000 0.000000041449 0.000000039205 0.000000090284 -0.000000002732 -0.000000025733 0.000000041449 0.000001329947
1 1645005919.404906749725 187 188 0.107178844297 -0.022750150514 -0.003656608090 0.000369946408 -0.000252619318 -0.007040696870 0.000000150210 -0.000000043366 0.000000003672 -0.000000019261 -0.000000023707 0.000000038513 -0.000000043366 0.000000022455 -0.000000001373 0.000000003685 0.000000010707 0.000000090631 0.000000003672 -0.000000001373 0.000000014393 -0.000000048027 -0.000000146585 -0.000000002253 -0.000000019261 0.000000003685 -0.000000048027 0.000000514919 0.000000488132 -0.000000028143 -0.000000023707 0.000000010707 -0.000000146585 0.000000488132 0.000001822771 0.000000035984 0.000000038513 0.000000090631 -0.000000002253 -0.000000028143 0.000000035984 0.000001341511
1 1645005919.509163618088 188 189 0.111763033254 -0.025831837093 -0.007938767615 0.000112630539 0.001169710732 -0.004771712646 0.000000150729 -0.000000043454 0.000000003900 -0.000000018668 -0.000000025291 0.000000043404 -0.000000043454 0.000000022348 -0.000000001378 0.000000003081 0.000000010657 0.000000089362 0.000000003900 -0.000000001378 0.000000014579 -0.000000049376 -0.000000149721 -0.000000001575 -0.000000018668 0.000000003081 -0.000000049376 0.000000521537 0.000000508382 -0.000000033045 -0.000000025291 0.000000010657 -0.000000149721 0.000000508382 0.000001873121 0.000000031279 0.000000043404 0.000000089362 -0.000000001575 -0.000000033045 0.000000031279 0.000001360429
1 1645005919.606138467789 189 190 0.103050190411 -0.028243153856 0.009036138321 0.000299620151 -0.000460363273 -0.002126451387 0.000000151210 -0.000000043262 0.000000004079 -0.000000019984 -0.000000029692 0.000000051331 -0.000000043262 0.000000022105 -0.000000001496 0.000000003836 0.000000012813 0.000000087307 0.000000004079 -0.000000001496 0.000000014344 -0.000000049082 -0.000000148242 -0.000000002279 -0.000000019984 0.000000003836 -0.000000049082 0.000000523071 0.000000508358 -0.000000029835 -0.000000029692 0.000000012813 -0.000000148242 0.000000508358 0.000001871353 0.000000041535 0.000000051331 0.000000087307 -0.000000002279 -0.000000029835 0.000000041535 0.000001379713
1 1645005919.708493471146 190 191 0.110366530571 -0.029472021130 0.018614876772 0.000102110420 0.000167129598 0.001994724913 0.000000150740 -0.000000042621 0.000000004151 -0.000000020036 -0.000000029606 0.000000062230 -0.000000042621 0.000000021889 -0.000000001558 0.000000003923 0.000000013386 0.000000086272 0.000000004151 -0.000000001558 0.000000014122 -0.000000049518 -0.000000146966 -0.000000002704 -0.000000020036 0.000000003923 -0.000000049518 0.000000525905 0.000000515666 -0.000000029326 -0.000000029606 0.000000013386 -0.000000146966 0.000000515666 0.000001869683 0.000000049449 0.000000062230 0.000000086272 -0.000000002704 -0.000000029326 0.000000049449 0.000001414664
1 1645005919.793772935867 191 192 0.096165140501 -0.018685263170 0.011579795113 -0.000740821107 -0.001031369105 0.004911205760 0.000000150428 -0.000000041858 0.000000003727 -0.000000018419 -0.000000025185 0.000000073735 -0.000000041858 0.000000021544 -0.000000001374 0.000000003222 0.000000011562 0.000000084970 0.000000003727 -0.000000001374 0.000000014202 -0.000000051029 -0.000000149309 -0.000000002193 -0.000000018419 0.000000003222 -0.000000051029 0.000000531889 0.000000537344 -0.000000031680 -0.000000025185 0.000000011562 -0.000000149309 0.000000537344 0.000001910836 0.000000046007 0.000000073735 0.000000084970 -0.000000002193 -0.000000031680 0.000000046007 0.000001453265
1 1645005919.894334554672 192 193 0.117461686956 -0.016398117007 -0.001090784968 -0.000555133742 -0.000926537311 0.007563804792 0.000000148505 -0.000000040661 0.000000003635 -0.000000017091 -0.000000022791 0.000000084172 -0.000000040661 0.000000020845 -0.000000001307 0.000000002552 0.000000010455 0.000000081570 0.000000003635 -0.000000001307 0.000000014106 -0.000000051158 -0.000000149559 -0.000000001627 -0.000000017091 0.000000002552 -0.000000051158 0.000000525853 0.000000542509 -0.000000035899 -0.000000022791 0.000000010455 -0.000000149559 0.000000542509 0.000001922636 0.000000041308 0.000000084172 0.000000081570 -0.000000001627 -0.000000035899 0.000000041308 0.000001463420
1 1645005920.004122257233 193 194 0.127889889395 -0.014287189664 -0.018071779409 -0.000603843631 0.001403222454 0.004675372250 0.000000147336 -0.000000039680 0.000000003583 -0.000000015186 -0.000000019399 0.000000089698 -0.000000039680 0.000000020410 -0.000000001303 0.000000001664 0.000000009811 0.000000081704 0.000000003583 -0.000000001303 0.000000014293 -0.000000052484 -0.000000153094 -0.000000001827 -0.000000015186 0.000000001664 -0.000000052484 0.000000530498 0.000000561884 -0.000000039870 -0.000000019399 0.000000009811 -0.000000153094 0.000000561884 0.000001973145 0.000000046528 0.000000089698 0.000000081704 -0.000000001827 -0.000000039870 0.000000046528 0.000001493220
1 1645005920.104864120483 194 195 0.116168811852 -0.013109825790 -0.012929698648 -0.000005389645 0.000738108287 0.001144078207 0.000000149213 -0.000000040106 0.000000003646 -0.000000015092 -0.000000021339 0.000000090319 -0.000000040106 0.000000020455 -0.000000001340 0.000000001571 0.000000010648 0.000000081937 0.000000003646 -0.000000001340 0.000000014645 -0.000000054570 -0.000000158509 -0.000000002004 -0.000000015092 0.000000001571 -0.000000054570 0.000000553075 0.000000590499 -0.000000041701 -0.000000021339 0.000000010648 -0.000000158509 0.000000590499 0.000002057287 0.000000048248 0.000000090319 0.000000081937 -0.000000002004 -0.000000041701 0.000000048248 0.000001514765
1 1645005920.206741094589 195 196 0.116606271407 -0.013867701039 0.005805133595 0.000067675702 0.000577447854 -0.002394489670 0.000000149429 -0.000000040285 0.000000003646 -0.000000014456 -0.000000021481 0.000000087876 -0.000000040285 0.000000020545 -0.000000001350 0.000000001645 0.000000010775 0.000000083356 0.000000003646 -0.000000001350 0.000000014509 -0.000000053933 -0.000000159305 -0.000000002159 -0.000000014456 0.000000001645 -0.000000053933 0.000000554499 0.000000591697 -0.000000038609 -0.000000021481 0.000000010775 -0.000000159305 0.000000591697 0.000002096327 0.000000049808 0.000000087876 0.000000083356 -0.000000002159 -0.000000038609 0.000000049808 0.000001532263
1 1645005920.306432723999 196 197 0.118459510620 -0.015741762731 0.019941986760 -0.000233586310 -0.001780831484 -0.005334381966 0.000000148573 -0.000000040237 0.000000003215 -0.000000013105 -0.000000017644 0.000000084415 -0.000000040237 0.000000020546 -0.000000001290 0.000000001926 0.000000010386 0.000000084228 0.000000003215 -0.000000001290 0.000000014559 -0.000000054075 -0.000000162036 -0.000000003226 -0.000000013105 0.000000001926 -0.000000054075 0.000000552256 0.000000602098 -0.000000029736 -0.000000017644 0.000000010386 -0.000000162036 0.000000602098 0.000002151812 0.000000061306 0.000000084415 0.000000084228 -0.000000003226 -0.000000029736 0.000000061306 0.000001531623
1 1645005920.396054744720 197 198 0.108833768936 -0.014196265349 0.014957699203 0.000784882132 -0.000440774433 -0.007383134271 0.000000148605 -0.000000040325 0.000000003132 -0.000000012207 -0.000000013711 0.000000083131 -0.000000040325 0.000000020417 -0.000000001368 0.000000002111 0.000000010502 0.000000083174 0.000000003132 -0.000000001368 0.000000015049 -0.000000056948 -0.000000169586 -0.000000004564 -0.000000012207 0.000000002111 -0.000000056948 0.000000566345 0.000000643775 -0.000000023618 -0.000000013711 0.000000010502 -0.000000169586 0.000000643775 0.000002263566 0.000000078674 0.000000083131 0.000000083174 -0.000000004564 -0.000000023618 0.000000078674 0.000001518540
1 1645005920.505497217178 198 199 0.132822748645 -0.016271936175 -0.000340704148 0.000018558141 0.001135204195 -0.009919704392 0.000000147651 -0.000000040083 0.000000002848 -0.000000010888 -0.000000007401 0.000000081858 -0.000000040083 0.000000020309 -0.000000001301 0.000000001666 0.000000008869 0.000000083786 0.000000002848 -0.000000001301 0.000000015367 -0.000000058874 -0.000000175370 -0.000000004773 -0.000000010888 0.000000001666 -0.000000058874 0.000000570999 0.000000675795 -0.000000023893 -0.000000007401 0.000000008869 -0.000000175370 0.000000675795 0.000002351570 0.000000082187 0.000000081858 0.000000083786 -0.000000004773 -0.000000023893 0.000000082187 0.000001524068

272
examples/IncrementalFixedLagSmootherExample.cpp Normal file
View File

@ -0,0 +1,272 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2025, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file IncrementalFixedLagExample.cpp
* @brief Example of incremental fixed-lag smoother using real-world data.
* @author Xiangcheng Hu (xhubd@connect.ust.hk), Frank Dellaert, Kevin Doherty
* @date Janaury 15, 2025
*
* Key objectives:
* - Validate `IncrementalFixedLagSmoother` functionality with real-world data.
* - Showcase how setting `findUnusedFactorSlots = true` addresses the issue #1452 in GTSAM, ensuring
* that unused factor slots (nullptrs) are correctly released when old factors are marginalized.
*
* This example leverages pose measurements from a real scenario. The test data (named "IncrementalFixedLagSmootherTestData.txt") is
* based on the corridor_day sequence from the FusionPortable dataset (https://fusionportable.github.io/dataset/fusionportable/).
* - 1 prior factor derived from point cloud ICP alignment with a prior map.
* - 199 relative pose factors derived from FAST-LIO2 odometry.
*
* Data Format (IncrementalFixedLagSmootherTestData.txt):
* 1) PRIOR factor line:
* @code
* 0 timestamp key x y z roll pitch yaw cov_6x6
* @endcode
* - "0" indicates PRIOR factor.
* - "timestamp" is the observation time (in seconds).
* - "key" is the integer ID for the Pose3 variable.
* - (x, y, z, roll, pitch, yaw) define the pose.
* - "cov_6x6" is the full 6x6 covariance matrix (row-major).
*
* 2) BETWEEN factor line:
* @code
* 1 timestamp key1 key2 x y z roll pitch yaw cov_6x6
* @endcode
* - "1" indicates BETWEEN factor.
* - "timestamp" is the observation time (in seconds).
* - "key1" and "key2" are the integer IDs for the connected Pose3 variables.
* - (x, y, z, roll, pitch, yaw) define the relative pose between these variables.
* - "cov_6x6" is the full 6x6 covariance matrix (row-major).
*
* See also:
* - GTSAM Issue #1452: https://github.com/borglab/gtsam/issues/1452
*/
// STL
#include <iostream>
#include <string>
// GTSAM
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h> // for writeG2o
using namespace std;
using namespace gtsam;
// Shorthand for symbols
using symbol_shorthand::X; // Pose3 (x,y,z, roll, pitch, yaw)
// Factor types
enum FactorType {
PRIOR = 0,
BETWEEN = 1
};
typedef Eigen::Matrix<double, 6, 6> Matrix6;
/* ************************************************************************* */
/**
* @brief Read a 6x6 covariance matrix from an input string stream.
*
* @param iss Input string stream containing covariance entries.
* @return 6x6 covariance matrix.
*/
Matrix6 readCovarianceMatrix(istringstream &iss) {
Matrix6 cov;
for (int r = 0; r < 6; ++r) {
for (int c = 0; c < 6; ++c) {
iss >> cov(r, c);
}
}
return cov;
}
/* ************************************************************************* */
/**
* @brief Create a Pose3 object from individual pose parameters.
*
* @param x Translation in x
* @param y Translation in y
* @param z Translation in z
* @param roll Rotation about x-axis
* @param pitch Rotation about y-axis
* @param yaw Rotation about z-axis
* @return Constructed Pose3 object
*/
Pose3 createPose(double x, double y, double z, double roll, double pitch, double yaw) {
return Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
}
/* ************************************************************************* */
/**
* @brief Save the factor graph and estimates to a .g2o file (for visualization/debugging).
*
* @param graph The factor graph
* @param estimate Current estimates of all variables
* @param filename Base filename for saving
* @param iterCount Iteration count to differentiate successive outputs
*/
void saveG2oGraph(const NonlinearFactorGraph &graph, const Values &estimate,
const string &filename, int iterCount) {
// Create zero-padded iteration count
string countStr = to_string(iterCount);
string paddedCount = string(5 - countStr.length(), '0') + countStr;
string fullFilename = filename + "_" + paddedCount + ".g2o";
// Write graph and estimates to g2o file
writeG2o(graph, estimate, fullFilename);
cout << "\nSaved graph to: " << fullFilename << endl;
}
/* ************************************************************************* */
/**
* @brief Main function: Reads poses data from a text file and performs incremental fixed-lag smoothing.
*
* Data Flow:
* 1) Parse lines from "IncrementalFixedLagSmootherTestData.txt".
* 2) For each line:
* - If it's a PRIOR factor, add a PriorFactor with a specified pose and covariance.
* - If it's a BETWEEN factor, add a BetweenFactor with a relative pose and covariance.
* - Insert new variables with initial guesses into the current solution if they don't exist.
* 3) Update the fixed-lag smoother (with iSAM2 inside) to incrementally optimize and marginalize out old poses
* beyond the specified lag window.
* 4) Repeat until all lines are processed.
* 5) Save the resulting factor graph and estimate of the last sliding window to a .g2o file.
*/
int main() {
string factor_loc = findExampleDataFile("issue1452.txt");
ifstream factor_file(factor_loc.c_str());
cout << "Reading factors data file: " << factor_loc << endl;
// Configure ISAM2 parameters for the fixed-lag smoother
ISAM2Params isamParameters;
isamParameters.relinearizeThreshold = 0.1;
isamParameters.relinearizeSkip = 1;
// Important!!!!!! Key parameter to ensure old factors are released after marginalization
isamParameters.findUnusedFactorSlots = true;
// Initialize fixed-lag smoother with a 1-second lag window
const double lag = 1.0;
IncrementalFixedLagSmoother smoother(lag, isamParameters);
// Print the iSAM2 parameters (optional)
isamParameters.print();
// Containers for incremental updates
NonlinearFactorGraph newFactors;
Values newValues;
FixedLagSmoother::KeyTimestampMap newTimestamps;
// For tracking the latest estimate of all states in the sliding window
Values currentEstimate;
Pose3 lastPose;
// Read and process each line in the factor graph file
string line;
int lineCount = 0;
while (getline(factor_file, line)) {
if (line.empty()) continue; // Skip empty lines
cout << "\n======================== Processing line " << ++lineCount
<< " =========================" << endl;
istringstream iss(line);
int factorType;
iss >> factorType;
// Check if the factor is PRIOR or BETWEEN
if (factorType == PRIOR) {
/**
* Format: PRIOR factor
* factor_type timestamp key pose(x y z roll pitch yaw) cov(6x6)
*/
double timestamp;
int key;
double x, y, z, roll, pitch, yaw;
iss >> timestamp >> key >> x >> y >> z >> roll >> pitch >> yaw;
Pose3 pose = createPose(x, y, z, roll, pitch, yaw);
Matrix6 cov = readCovarianceMatrix(iss);
auto noise = noiseModel::Gaussian::Covariance(cov);
// Add prior factor
newFactors.addPrior(X(key), pose, noise);
cout << "Add PRIOR factor on key " << key << endl;
// Provide initial guess if not already in the graph
if (!newValues.exists(X(key))) {
newValues.insert(X(key), pose);
newTimestamps[X(key)] = timestamp;
}
} else if (factorType == BETWEEN) {
/**
* Format: BETWEEN factor
* factor_type timestamp key1 key2 pose(x y z roll pitch yaw) cov(6x6)
*/
double timestamp;
int key1, key2;
iss >> timestamp >> key1 >> key2;
double x1, y1, z1, roll1, pitch1, yaw1;
iss >> x1 >> y1 >> z1 >> roll1 >> pitch1 >> yaw1;
Pose3 relativePose = createPose(x1, y1, z1, roll1, pitch1, yaw1);
Matrix6 cov = readCovarianceMatrix(iss);
auto noise = noiseModel::Gaussian::Covariance(cov);
// Add between factor
newFactors.emplace_shared<BetweenFactor<Pose3>>(X(key1), X(key2), relativePose, noise);
cout << "Add BETWEEN factor: " << key1 << " -> " << key2 << endl;
// Provide an initial guess if needed
if (!newValues.exists(X(key2))) {
newValues.insert(X(key2), lastPose.compose(relativePose));
newTimestamps[X(key2)] = timestamp;
}
} else {
cerr << "Unknown factor type: " << factorType << endl;
continue;
}
// Print some intermediate statistics
cout << "Before update - Graph has " << smoother.getFactors().size()
<< " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl;
cout << "New factors: " << newFactors.size()
<< ", New values: " << newValues.size() << endl;
// Attempt to update the smoother with new factors and values
try {
smoother.update(newFactors, newValues, newTimestamps);
// Optional: Perform extra internal iterations if needed
size_t maxExtraIterations = 3;
for (size_t i = 1; i < maxExtraIterations; ++i) {
smoother.update();
}
// you may not get expected results if you use the gtsam version lower than 4.3
cout << "After update - Graph has " << smoother.getFactors().size()
<< " factors, " << smoother.getFactors().nrFactors() << " nr factors." << endl;
// Retrieve the latest estimate
currentEstimate = smoother.calculateEstimate();
if (!currentEstimate.empty()) {
// Update lastPose to the last key's estimate
Key lastKey = currentEstimate.keys().back();
lastPose = currentEstimate.at<Pose3>(lastKey);
}
// Clear containers for the next iteration
newFactors.resize(0);
newValues.clear();
newTimestamps.clear();
} catch (const exception &e) {
cerr << "Smoother update failed: " << e.what() << endl;
}
}
// The results of the last sliding window are saved to a g2o file for visualization or further analysis.
saveG2oGraph(smoother.getFactors(), currentEstimate, "isam", lineCount);
factor_file.close();
return 0;
}

1
gtsam/CMakeLists.txt
View File

@ -6,6 +6,7 @@ project(gtsam LANGUAGES CXX)
set (gtsam_subdirs
base
basis
constrained
geometry
inference
symbolic

3
gtsam/base/std_optional_serialization.h
View File

@ -48,7 +48,8 @@
*/
#ifdef __GNUC__
#if __GNUC__ >= 7 && __cplusplus >= 201703L
namespace boost { namespace serialization { struct U; } }
// Based on https://github.com/borglab/gtsam/issues/1738, we define U as a complete type.
namespace boost { namespace serialization { struct U{}; } }
namespace std { template<> struct is_trivially_default_constructible<boost::serialization::U> : std::false_type {}; }
namespace std { template<> struct is_trivially_copy_constructible<boost::serialization::U> : std::false_type {}; }
namespace std { template<> struct is_trivially_move_constructible<boost::serialization::U> : std::false_type {}; }

2
gtsam/base/tests/testStdOptionalSerialization.cpp
View File

@ -60,8 +60,6 @@ public:
TestOptionalStruct() = default;
TestOptionalStruct(const int& opt)
: opt(opt) {}
// A copy constructor is needed for serialization
TestOptionalStruct(const TestOptionalStruct& other) = default;
bool operator==(const TestOptionalStruct& other) const {
// check the values are equal
return *opt == *other.opt;

4
gtsam/base/timing.cpp
View File

@ -109,7 +109,7 @@ void TimingOutline::print(const std::string& outline) const {
/* ************************************************************************* */
void TimingOutline::printCsvHeader(bool addLineBreak) const {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
// Order is (CPU time, number of times, wall time, time + children in seconds,
// min time, max time)
std::cout << label_ + " cpu time (s)" << "," << label_ + " #calls" << ","
@ -134,7 +134,7 @@ void TimingOutline::printCsvHeader(bool addLineBreak) const {
/* ************************************************************************* */
void TimingOutline::printCsv(bool addLineBreak) const {
#ifdef GTSAM_USE_BOOST_FEATURES
#if GTSAM_USE_BOOST_FEATURES
// Order is (CPU time, number of times, wall time, time + children in seconds,
// min time, max time)
std::cout << self() << "," << n_ << "," << wall() << "," << secs() << ","

6
gtsam/constrained/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# Install headers
file(GLOB constraint_headers "*.h")
install(FILES ${constraint_headers} DESTINATION include/gtsam/constrained)
# Build tests
add_subdirectory(tests)

97
gtsam/constrained/InequalityPenaltyFunction.cpp Normal file
View File

@ -0,0 +1,97 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file InequalityPenaltyFunction.cpp
* @brief Ramp function to compute inequality constraint violations.
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#include <gtsam/constrained/InequalityPenaltyFunction.h>
namespace gtsam {
/* ************************************************************************* */
InequalityPenaltyFunction::UnaryScalarFunc InequalityPenaltyFunction::function()
const {
return [this](const double& x, OptionalJacobian<1, 1> H = {}) {
return this->operator()(x, H);
};
}
/* ************************************************************************* */
double RampFunction::Ramp(const double x, OptionalJacobian<1, 1> H) {
if (x < 0) {
if (H) {
H->setConstant(0);
}
return 0;
} else {
if (H) {
H->setConstant(1);
}
return x;
}
}
/* ************************************************************************* */
double SmoothRampPoly2::operator()(const double& x,
OptionalJacobian<1, 1> H) const {
if (x <= 0) {
if (H) {
H->setZero();
}
return 0;
} else if (x < epsilon_) {
if (H) {
H->setConstant(2 * a_ * x);
}
return a_ * pow(x, 2);
} else {
if (H) {
H->setOnes();
}
return x - epsilon_ / 2;
}
}
/* ************************************************************************* */
double SmoothRampPoly3::operator()(const double& x,
OptionalJacobian<1, 1> H) const {
if (x <= 0) {
if (H) {
H->setZero();
}
return 0;
} else if (x < epsilon_) {
if (H) {
H->setConstant(3 * a_ * pow(x, 2) + 2 * b_ * x);
}
return a_ * pow(x, 3) + b_ * pow(x, 2);
} else {
if (H) {
H->setOnes();
}
return x;
}
}
/* ************************************************************************* */
double SoftPlusFunction::operator()(const double& x,
OptionalJacobian<1, 1> H) const {
if (H) {
H->setConstant(1 / (1 + std::exp(-k_ * x)));
}
return std::log(1 + std::exp(k_ * x)) / k_;
}
} // namespace gtsam

149
gtsam/constrained/InequalityPenaltyFunction.h Normal file
View File

@ -0,0 +1,149 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file InequalityPenaltyFunction.h
* @brief Ramp function to compute inequality constraint violations.
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressions.h>
namespace gtsam {
/** Base class for smooth approximation of the ramp function. */
class GTSAM_EXPORT InequalityPenaltyFunction {
public:
typedef std::shared_ptr<InequalityPenaltyFunction> shared_ptr;
typedef std::function<double(const double& x, OptionalJacobian<1, 1> H)>
UnaryScalarFunc;
/** Constructor. */
InequalityPenaltyFunction() {}
/** Destructor. */
virtual ~InequalityPenaltyFunction() {}
virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const = 0;
virtual UnaryScalarFunc function() const;
};
/** Ramp function f : R -> R.
* f(x) = 0 for x <= 0
* x for x > 0
*/
class GTSAM_EXPORT RampFunction : public InequalityPenaltyFunction {
public:
typedef InequalityPenaltyFunction Base;
typedef RampFunction This;
typedef std::shared_ptr<This> shared_ptr;
public:
RampFunction() : Base() {}
virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override {
return Ramp(x, H);
}
virtual UnaryScalarFunc function() const override { return Ramp; }
static double Ramp(const double x, OptionalJacobian<1, 1> H = {});
};
/** Ramp function approximated with a polynomial of degree 2 in [0, epsilon].
* The coefficients are computed as
* a = 1 / (2 * epsilon)
* Function f(x) = 0 for x <= 0
* a * x^2 for 0 < x < epsilon
* x - epsilon/2 for x >= epsilon
*/
class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction {
public:
typedef InequalityPenaltyFunction Base;
typedef SmoothRampPoly2 This;
typedef std::shared_ptr<This> shared_ptr;
protected:
double epsilon_;
double a_;
public:
/** Constructor.
* @param epsilon parameter for adjusting the smoothness of the function.
*/
SmoothRampPoly2(const double epsilon = 1)
: Base(), epsilon_(epsilon), a_(0.5 / epsilon) {}
virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override;
};
/** Ramp function approximated with a polynomial of degree 3 in [0, epsilon].
* The coefficients are computed as
* a = -1 / epsilon^2
* b = 2 / epsilon
* Function f(x) = 0 for x <= 0
* a * x^3 + b * x^2 for 0 < x < epsilon
* x for x >= epsilon
*/
class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction {
public:
typedef InequalityPenaltyFunction Base;
typedef SmoothRampPoly3 This;
typedef std::shared_ptr<This> shared_ptr;
protected:
double epsilon_;
double a_;
double b_;
public:
/** Constructor.
* @param epsilon parameter for adjusting the smoothness of the function.
*/
SmoothRampPoly3(const double epsilon = 1)
: Base(),
epsilon_(epsilon),
a_(-1 / (epsilon * epsilon)),
b_(2 / epsilon) {}
virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override;
};
/** Softplus function that implements f(x) = log(1 + exp(k*x)) / k. */
class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction {
public:
typedef InequalityPenaltyFunction Base;
typedef SoftPlusFunction This;
typedef std::shared_ptr<This> shared_ptr;
protected:
double k_;
public:
/** Constructor.
* @param k parameter for adjusting the smoothness of the function.
*/
SoftPlusFunction(const double k = 1) : Base(), k_(k) {}
virtual double operator()(const double& x,
OptionalJacobian<1, 1> H = {}) const override;
};
} // namespace gtsam

91
gtsam/constrained/NonlinearConstraint.h Normal file
View File

@ -0,0 +1,91 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NonlinearConstraint.h
* @brief Nonlinear constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024
*/
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
namespace gtsam {
/**
* Base class for nonlinear constraint.
* The constraint is represented as a NoiseModelFactor with Constrained noise model.
* whitenedError() returns The constraint violation vector.
* unwhitenedError() returns the sigma-scaled constraint violation vector.
*/
class GTSAM_EXPORT NonlinearConstraint : public NoiseModelFactor {
public:
typedef NoiseModelFactor Base;
/** Use constructors of NoiseModelFactor. */
using Base::Base;
using NonlinearFactor::error;
/** Destructor. */
virtual ~NonlinearConstraint() {}
/** Create a cost factor representing the L2 penalty function with scaling coefficient mu. */
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const {
return cloneWithNewNoiseModel(penaltyNoise(mu));
}
/** Return the norm of the constraint violation vector. */
virtual double violation(const Values& x) const { return sqrt(2 * error(x)); }
/** Check if constraint violation is within tolerance. */
virtual bool feasible(const Values& x, const double tolerance = 1e-5) const {
return violation(x) <= tolerance;
}
const Vector sigmas() const { return noiseModel()->sigmas(); }
// return the hessian of unwhitened error function in each dimension.
virtual std::vector<Matrix> unwhitenedHessian(const Values& x) const {
throw std::runtime_error("hessian not implemented");
}
protected:
/** Noise model used for the penalty function. */
SharedDiagonal penaltyNoise(const double mu) const {
return noiseModel::Diagonal::Sigmas(noiseModel()->sigmas() / sqrt(mu));
}
/** Default constrained noisemodel used for construction of constraint. */
static SharedNoiseModel constrainedNoise(const Vector& sigmas) {
return noiseModel::Constrained::MixedSigmas(1.0, sigmas);
}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("NonlinearConstraint",
boost::serialization::base_object<Base>(*this));
}
#endif
};
} // namespace gtsam

54
gtsam/constrained/NonlinearEqualityConstraint-inl.h Normal file
View File

@ -0,0 +1,54 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NonlinearEqualityConstraint-inl.h
* @brief Nonlinear equality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */
#pragma once
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
namespace gtsam {
/* ********************************************************************************************* */
template <typename T>
ExpressionEqualityConstraint<T>::ExpressionEqualityConstraint(const Expression<T>& expression,
const T& rhs,
const Vector& sigmas)
: Base(constrainedNoise(sigmas), expression.keysAndDims().first),
expression_(expression),
rhs_(rhs),
dims_(expression.keysAndDims().second) {}
/* ********************************************************************************************* */
template <typename T>
Vector ExpressionEqualityConstraint<T>::unwhitenedError(const Values& x,
OptionalMatrixVecType H) const {
// Copy-paste from ExpressionFactor.
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
return -traits<T>::Local(value, rhs_);
} else {
const T value = expression_.value(x);
return -traits<T>::Local(value, rhs_);
}
}
/* ********************************************************************************************* */
template <typename T>
NoiseModelFactor::shared_ptr ExpressionEqualityConstraint<T>::penaltyFactor(const double mu) const {
return std::make_shared<ExpressionFactor<T>>(penaltyNoise(mu), rhs_, expression_);
}
} // namespace gtsam

84
gtsam/constrained/NonlinearEqualityConstraint.cpp Normal file
View File

@ -0,0 +1,84 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NonlinearEqualityConstraint.cpp
* @brief Nonlinear equality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/inference/FactorGraph-inst.h>
namespace gtsam {
/* ********************************************************************************************* */
ZeroCostConstraint::ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor)
: Base(constrainedNoise(factor->noiseModel()->sigmas()), factor->keys()), factor_(factor) {}
/* ********************************************************************************************* */
Vector ZeroCostConstraint::unwhitenedError(const Values& x, OptionalMatrixVecType H) const {
return factor_->unwhitenedError(x, H);
}
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ZeroCostConstraint::penaltyFactor(const double mu) const {
return factor_->cloneWithNewNoiseModel(penaltyNoise(mu));
}
/* ********************************************************************************************* */
NonlinearEqualityConstraints NonlinearEqualityConstraints::FromCostGraph(
const NonlinearFactorGraph& graph) {
NonlinearEqualityConstraints constraints;
for (const auto& factor : graph) {
auto noise_factor = std::dynamic_pointer_cast<NoiseModelFactor>(factor);
constraints.emplace_shared<ZeroCostConstraint>(noise_factor);
}
return constraints;
}
/* ********************************************************************************************* */
size_t NonlinearEqualityConstraints::dim() const {
size_t dimension = 0;
for (const auto& constraint : *this) {
dimension += constraint->dim();
}
return dimension;
}
/* ********************************************************************************************* */
Vector NonlinearEqualityConstraints::violationVector(const Values& values, bool whiten) const {
Vector violation(dim());
size_t start_idx = 0;
for (const auto& constraint : *this) {
size_t dim = constraint->dim();
violation.middleCols(start_idx, dim) =
whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
start_idx += dim;
}
return violation;
}
/* ********************************************************************************************* */
double NonlinearEqualityConstraints::violationNorm(const Values& values) const {
return violationVector(values, true).norm();
}
/* ********************************************************************************************* */
NonlinearFactorGraph NonlinearEqualityConstraints::penaltyGraph(const double mu) const {
NonlinearFactorGraph graph;
for (const auto& constraint : *this) {
graph.add(constraint->penaltyFactor(mu));
}
return graph;
}
} // namespace gtsam

182
gtsam/constrained/NonlinearEqualityConstraint.h Normal file
View File

@ -0,0 +1,182 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NonlinearEqualityConstraint.h
* @brief Nonlinear equality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */
#pragma once
#include <gtsam/constrained/NonlinearConstraint.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
/**
* Equality constraint base class.
*/
class GTSAM_EXPORT NonlinearEqualityConstraint : public NonlinearConstraint {
public:
typedef NonlinearConstraint Base;
typedef NonlinearEqualityConstraint This;
typedef std::shared_ptr<This> shared_ptr;
/** Default constructor. */
using Base::Base;
/** Destructor. */
virtual ~NonlinearEqualityConstraint() {}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("NonlinearEqualityConstraint",
boost::serialization::base_object<Base>(*this));
}
#endif
};
/** Equality constraint that force g(x) = M. */
template <typename T>
class ExpressionEqualityConstraint : public NonlinearEqualityConstraint {
public:
typedef NonlinearEqualityConstraint Base;
typedef ExpressionEqualityConstraint This;
typedef std::shared_ptr<This> shared_ptr;
protected:
Expression<T> expression_;
T rhs_;
FastVector<int> dims_;
public:
/**
* @brief Constructor.
*
* @param expression expression representing g(x).
* @param tolerance vector representing tolerance in each dimension.
*/
ExpressionEqualityConstraint(const Expression<T>& expression, const T& rhs, const Vector& sigmas);
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
const Expression<T>& expression() const { return expression_; }
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("ExpressionEqualityConstraint",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(expression_);
ar& BOOST_SERIALIZATION_NVP(rhs_);
ar& BOOST_SERIALIZATION_NVP(dims_);
}
#endif
};
/** Equality constraint that enforce the cost factor with zero error.
* e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the
* equlity 2x-1=0.
*/
class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint {
public:
typedef NonlinearEqualityConstraint Base;
typedef ZeroCostConstraint This;
typedef std::shared_ptr<This> shared_ptr;
protected:
NoiseModelFactor::shared_ptr factor_;
public:
/**
* @brief Constructor.
*
* @param factor NoiseModel factor.
*/
ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor);
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
virtual NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("ZeroCostConstraint",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(factor_);
}
#endif
};
/// Container of NonlinearEqualityConstraint.
class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph<NonlinearEqualityConstraint> {
public:
typedef std::shared_ptr<NonlinearEqualityConstraints> shared_ptr;
typedef FactorGraph<NonlinearEqualityConstraint> Base;
public:
using Base::Base;
/// Create constraints ensuring the cost of factors of a graph is zero.
static NonlinearEqualityConstraints FromCostGraph(const NonlinearFactorGraph& graph);
size_t dim() const;
/// Evaluate the constraint violation as a vector
Vector violationVector(const Values& values, bool whiten = true) const;
/// Evaluate the constraint violation (as 2-norm of the violation vector).
double violationNorm(const Values& values) const;
NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("NonlinearEqualityConstraints",
boost::serialization::base_object<Base>(*this));
}
#endif
};
} // namespace gtsam
#include <gtsam/constrained/NonlinearEqualityConstraint-inl.h>

179
gtsam/constrained/NonlinearInequalityConstraint.cpp Normal file
View File

@ -0,0 +1,179 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequalityConstraint.cpp
* @brief Nonlinear inequality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024
*/
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
#include <gtsam/inference/FactorGraph-inst.h>
namespace gtsam {
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraint::whitenedExpr(const Values& x) const {
return noiseModel()->whiten(unwhitenedExpr(x));
}
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraint::unwhitenedError(const Values& x,
OptionalMatrixVecType H) const {
Vector error = unwhitenedExpr(x, H);
for (size_t i = 0; i < dim(); i++) {
if (error(i) < 0) {
error(i) = 0;
if (H) {
for (Matrix& m : *H) {
m.row(i).setZero();
}
}
}
}
return error;
}
/* ********************************************************************************************* */
bool NonlinearInequalityConstraint::active(const Values& x) const {
return (unwhitenedExpr(x).array() >= 0).any();
}
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorCustom(
InequalityPenaltyFunction::shared_ptr func, const double mu) const {
/// Default behavior, this function should be overriden.
return penaltyFactor(mu);
}
/* ********************************************************************************************* */
NonlinearEqualityConstraint::shared_ptr NonlinearInequalityConstraint::createEqualityConstraint()
const {
/// Default behavior, this function should be overriden.
return nullptr;
}
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr NonlinearInequalityConstraint::penaltyFactorEquality(
const double mu) const {
return createEqualityConstraint()->penaltyFactor(mu);
}
/* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::ScalarExpressionInequalityConstraint(
const Double_& expression, const double& sigma)
: Base(constrainedNoise(Vector1(sigma)), expression.keysAndDims().first),
expression_(expression),
dims_(expression.keysAndDims().second) {}
/* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::GeqZero(
const Double_& expression, const double& sigma) {
Double_ neg_expr = Double_(0.0) - expression;
return std::make_shared<ScalarExpressionInequalityConstraint>(neg_expr, sigma);
}
/* ********************************************************************************************* */
ScalarExpressionInequalityConstraint::shared_ptr ScalarExpressionInequalityConstraint::LeqZero(
const Double_& expression, const double& sigma) {
return std::make_shared<ScalarExpressionInequalityConstraint>(expression, sigma);
}
/* ********************************************************************************************* */
Vector ScalarExpressionInequalityConstraint::unwhitenedExpr(const Values& x,
OptionalMatrixVecType H) const {
// Copy-paste from ExpressionFactor.
if (H) {
return Vector1(expression_.valueAndDerivatives(x, keys_, dims_, *H));
} else {
return Vector1(expression_.value(x));
}
}
/* ********************************************************************************************* */
NonlinearEqualityConstraint::shared_ptr
ScalarExpressionInequalityConstraint::createEqualityConstraint() const {
return std::make_shared<ExpressionEqualityConstraint<double>>(
expression_, 0.0, noiseModel()->sigmas());
}
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactor(
const double mu) const {
Double_ penalty_expression(RampFunction::Ramp, expression_);
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, penalty_expression);
}
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorCustom(
InequalityPenaltyFunction::shared_ptr func, const double mu) const {
if (!func) {
return penaltyFactor(mu);
}
// TODO(yetong): can we pass the functor directly to construct the expression?
Double_ error(func->function(), expression_);
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, error);
}
/* ********************************************************************************************* */
NoiseModelFactor::shared_ptr ScalarExpressionInequalityConstraint::penaltyFactorEquality(
const double mu) const {
return std::make_shared<ExpressionFactor<double>>(penaltyNoise(mu), 0.0, expression_);
}
/* ********************************************************************************************* */
size_t NonlinearInequalityConstraints::dim() const {
size_t dimension = 0;
for (const auto& constraint : *this) {
dimension += constraint->dim();
}
return dimension;
}
/* ********************************************************************************************* */
Vector NonlinearInequalityConstraints::violationVector(const Values& values, bool whiten) const {
Vector violation(dim());
size_t start_idx = 0;
for (const auto& constraint : *this) {
size_t dim = constraint->dim();
violation.middleCols(start_idx, dim) =
whiten ? constraint->whitenedError(values) : constraint->unwhitenedError(values);
start_idx += dim;
}
return violation;
}
/* ********************************************************************************************* */
double NonlinearInequalityConstraints::violationNorm(const Values& values) const {
return violationVector(values, true).norm();
}
/* ********************************************************************************************* */
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraph(const double mu) const {
NonlinearFactorGraph graph;
for (const auto& constraint : *this) {
graph.add(constraint->penaltyFactor(mu));
}
return graph;
}
/* ********************************************************************************************* */
NonlinearFactorGraph NonlinearInequalityConstraints::penaltyGraphCustom(
InequalityPenaltyFunction::shared_ptr func, const double mu) const {
NonlinearFactorGraph graph;
for (const auto& constraint : *this) {
graph.add(constraint->penaltyFactorCustom(func, mu));
}
return graph;
}
} // namespace gtsam

183
gtsam/constrained/NonlinearInequalityConstraint.h Normal file
View File

@ -0,0 +1,183 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NonlinearInequalityConstraint.h
* @brief Nonlinear inequality constraints in constrained optimization.
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024
*/
#pragma once
#include <gtsam/constrained/InequalityPenaltyFunction.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/nonlinear/expressions.h>
namespace gtsam {
/**
* Inequality constraint base class, enforcing g(x) <= 0.
*/
class GTSAM_EXPORT NonlinearInequalityConstraint : public NonlinearConstraint {
public:
typedef NonlinearConstraint Base;
typedef NonlinearInequalityConstraint This;
typedef std::shared_ptr<This> shared_ptr;
/** Default constructor. */
using Base::Base;
/** Destructor. */
virtual ~NonlinearInequalityConstraint() {}
/** Return g(x). */
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const = 0;
virtual Vector whitenedExpr(const Values& x) const;
/** Return ramp(g(x)). */
virtual Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override;
/** Return true if g(x)>=0 in any dimension. */
virtual bool active(const Values& x) const override;
/** Return an equality constraint corresponding to g(x)=0. */
virtual NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const;
/** Cost factor using a customized penalty function. */
virtual NoiseModelFactor::shared_ptr penaltyFactorCustom(
InequalityPenaltyFunction::shared_ptr func, const double mu = 1.0) const;
/** penalty function as if the constraint is equality, 0.5 * mu * ||g(x)||^2 */
virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const;
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("NonlinearInequalityConstraint",
boost::serialization::base_object<Base>(*this));
}
#endif
};
/** Inequality constraint that force g(x) <= 0, where g(x) is a scalar-valued
* nonlinear function.
*/
class GTSAM_EXPORT ScalarExpressionInequalityConstraint : public NonlinearInequalityConstraint {
public:
typedef NonlinearInequalityConstraint Base;
typedef ScalarExpressionInequalityConstraint This;
typedef std::shared_ptr<This> shared_ptr;
protected:
Double_ expression_;
FastVector<int> dims_;
public:
/**
* @brief Constructor.
*
* @param expression expression representing g(x) (or -g(x) for GeqZero).
* @param sigma scalar representing sigma.
*/
ScalarExpressionInequalityConstraint(const Double_& expression, const double& sigma);
/** Create an inequality constraint g(x)/sigma >= 0, internally represented as -g(x)/sigma <= 0.
*/
static ScalarExpressionInequalityConstraint::shared_ptr GeqZero(const Double_& expression,
const double& sigma);
/** Create an inequality constraint g(x)/sigma <= 0. */
static ScalarExpressionInequalityConstraint::shared_ptr LeqZero(const Double_& expression,
const double& sigma);
/** Compute g(x), or -g(x) for objects constructed from GeqZero. */
virtual Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override;
/** Equality constraint representing g(x)/sigma = 0. */
NonlinearEqualityConstraint::shared_ptr createEqualityConstraint() const override;
/** Penalty function 0.5*mu*||ramp(g(x)/sigma||^2. */
NoiseModelFactor::shared_ptr penaltyFactor(const double mu = 1.0) const override;
/** Penalty function using a smooth approxiamtion of the ramp funciton. */
NoiseModelFactor::shared_ptr penaltyFactorCustom(InequalityPenaltyFunction::shared_ptr func,
const double mu = 1.0) const override;
/** Penalty function as if the constraint is equality, 0.5 * mu * ||g(x)/sigma||^2. */
virtual NoiseModelFactor::shared_ptr penaltyFactorEquality(const double mu = 1.0) const override;
/** Return expression g(x), or -g(x) for objects constructed from GeqZero. */
const Double_& expression() const { return expression_; }
/** return a deep copy of this factor. */
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("ExpressionEqualityConstraint",
boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(expression_);
ar& BOOST_SERIALIZATION_NVP(dims_);
}
#endif
};
/// Container of NonlinearInequalityConstraint.
class GTSAM_EXPORT NonlinearInequalityConstraints
: public FactorGraph<NonlinearInequalityConstraint> {
public:
typedef FactorGraph<NonlinearInequalityConstraint> Base;
typedef NonlinearInequalityConstraints This;
typedef std::shared_ptr<This> shared_ptr;
using Base::Base;
/** Return the total dimension of constraints. */
size_t dim() const;
/** Evaluate the constraint violation as a vector. */
Vector violationVector(const Values& values, bool whiten = true) const;
/** Evaluate the constraint violation (as L2 norm). */
double violationNorm(const Values& values) const;
/** Return the penalty function corresponding to \sum_i||ramp(g_i(x))||^2. */
NonlinearFactorGraph penaltyGraph(const double mu = 1.0) const;
/** Return the cost graph constructed using a customized penalty function. */
NonlinearFactorGraph penaltyGraphCustom(InequalityPenaltyFunction::shared_ptr func,
const double mu = 1.0) const;
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("NonlinearInequalityConstraints",
boost::serialization::base_object<Base>(*this));
}
#endif
};
} // namespace gtsam

9
gtsam/constrained/tests/CMakeLists.txt Normal file
View File

@ -0,0 +1,9 @@
# if GTSAM_ENABLE_BOOST_SERIALIZATION is OFF then exclude some tests
if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
# create a semicolon seperated list of files to exclude
set(EXCLUDE_TESTS "testSerializationConstraint.cpp")
else()
set(EXCLUDE_TESTS "")
endif()
gtsamAddTestsGlob(constraint "test*.cpp" "${EXCLUDE_TESTS}" "gtsam")

73
gtsam/constrained/tests/constrainedExample.h Normal file
View File

@ -0,0 +1,73 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file constrainedExample.h
* @brief Simple constrained optimization scenarios.
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#pragma once
// #include <gtsam/constrained/ConstrainedOptProblem.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
namespace constrained_example {
using namespace gtsam;
/// Exponential function e^x.
inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) {
double result = exp(x);
if (H1) H1->setConstant(result);
return result;
}
/// Exponential expression e^x.
inline Double_ exp(const Double_& x) { return Double_(exp_func, x); }
/// Pow functor used for pow function.
class PowFunctor {
private:
double c_;
public:
PowFunctor(const double& c) : c_(c) {}
double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const {
if (H1) H1->setConstant(c_ * pow(x, c_ - 1));
return pow(x, c_);
}
};
/// Pow function.
inline Double_ pow(const Double_& x, const double& c) {
PowFunctor pow_functor(c);
return Double_(pow_functor, x);
}
/// Plus between Double expression and double.
inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); }
/// Negative sign operator.
inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; }
/// Keys for creating expressions.
Symbol x1_key('x', 1);
Symbol x2_key('x', 2);
Double_ x1(x1_key), x2(x2_key);
} // namespace constrained_example

128
gtsam/constrained/tests/testInequalityPenaltyFunction.cpp Normal file
View File

@ -0,0 +1,128 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testInequalityPenaltyFunction.h
* @brief unit tests for ramp functions
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constrained/InequalityPenaltyFunction.h>
using namespace gtsam;
/* ********************************************************************************************* */
TEST(RampFunction, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
auto ramp_helper = [&](const double& x) { return RampFunction::Ramp(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{0.0, 0.0, 1.0, 2.0, 3.0};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = RampFunction::Ramp(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
/// Check derivative.
if (abs(x) > 1e-6) { // function is not smooth at 0, so Jacobian is undefined.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H));
}
}
}
/* ********************************************************************************************* */
TEST(RampFunctionPoly2, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
SmoothRampPoly2 p_ramp(2.0);
auto ramp_helper = [&](const double& x) { return p_ramp(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{0.0, 0.0, 0.25, 1.0, 2.0};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = p_ramp(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
/// Check derivative.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H, 1e-6));
}
}
/* ********************************************************************************************* */
TEST(RampFunctionPoly3, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
SmoothRampPoly3 p_ramp(2.0);
auto ramp_helper = [&](const double& x) { return p_ramp(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{0.0, 0.0, 0.75, 2.0, 3.0};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = p_ramp(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-9);
/// Check derivative.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(ramp_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H, 1e-6));
}
}
/* ********************************************************************************************* */
TEST(SoftPlusFunction, error_and_jacobian) {
/// Helper function for numerical Jacobian computation.
SoftPlusFunction soft_plus(0.5);
auto soft_plus_helper = [&](const double& x) { return soft_plus(x); };
/// Create a set of values to test the function.
static std::vector<double> x_vec{-3.0, 0.0, 1.0, 2.0, 3.0};
static std::vector<double> expected_r_vec{
0.40282656, 1.38629436, 1.94815397, 2.62652338, 3.40282656};
for (size_t i = 0; i < x_vec.size(); i++) {
double x = x_vec.at(i);
Matrix H;
double r = soft_plus(x, H);
/// Check function evaluation.
EXPECT_DOUBLES_EQUAL(expected_r_vec.at(i), r, 1e-6);
/// Check derivative.
Matrix expected_H = gtsam::numericalDerivative11<double, double, 1>(soft_plus_helper, x, 1e-6);
EXPECT(assert_equal(expected_H, H, 1e-6));
}
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

259
gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp Normal file
View File

@ -0,0 +1,259 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testNonlinearEqualityConstraint.cpp
* @brief unit tests for nonlinear equality constraints
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#include <CppUnitLite/Test.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>
#include "constrainedExample.h"
using namespace gtsam;
using constrained_example::pow;
using constrained_example::x1, constrained_example::x2;
using constrained_example::x1_key, constrained_example::x2_key;
// Test methods of DoubleExpressionEquality.
TEST(ExpressionEqualityConstraint, double) {
// create constraint from double expression
// g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
Vector sigmas = Vector1(0.1);
auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
auto constraint = ExpressionEqualityConstraint<double>(g, 0.0, sigmas);
EXPECT(constraint.noiseModel()->isConstrained());
EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
// Create 2 sets of values for testing.
Values values1, values2;
values1.insert(x1_key, 0.0);
values1.insert(x2_key, 0.0);
values2.insert(x1_key, 1.0);
values2.insert(x2_key, 1.0);
// Check that values1 are feasible.
EXPECT(constraint.feasible(values1));
// Check that violation evaluates as 0 at values1.
EXPECT(assert_equal(Vector::Zero(1), constraint.unwhitenedError(values1)));
EXPECT(assert_equal(Vector::Zero(1), constraint.whitenedError(values1)));
EXPECT(assert_equal(0.0, constraint.error(values1)));
// Check that values2 are indeed deemed infeasible.
EXPECT(!constraint.feasible(values2));
// Check constraint violation is indeed g(x) at values2.
EXPECT(assert_equal(Vector::Constant(1, 4.0), constraint.unwhitenedError(values2)));
EXPECT(assert_equal(Vector::Constant(1, 40), constraint.whitenedError(values2)));
EXPECT(assert_equal(800, constraint.error(values2)));
// Check dimension is 1 for scalar g.
EXPECT(constraint.dim() == 1);
// Check keys include x1, x2.
EXPECT(constraint.keys().size() == 2);
EXPECT(x1_key == *constraint.keys().begin());
EXPECT(x2_key == *constraint.keys().rbegin());
// Generate factor representing the term in merit function.
double mu = 4;
auto merit_factor = constraint.penaltyFactor(mu);
// Check that noise model sigma == sigmas/sqrt(mu).
auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
// Check that error is equal to 0.5*mu * (g(x)+bias)^2/sigmas^2.
double expected_error1 = 0; // 0.5 * 4 * ||0||_(0.1^2)^2
EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
double expected_error2 = 3200; // 0.5 * 4 * ||4||_(0.1^2)^2
EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
// Check jacobian computation is correct.
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
}
// Test methods of VectorExpressionEquality.
TEST(ExpressionEqualityConstraint, Vector2) {
// g(v1, v2) = v1 + v2, our own example.
Vector2_ x1_vec_expr(x1_key);
Vector2_ x2_vec_expr(x2_key);
auto g = x1_vec_expr + x2_vec_expr;
auto sigmas = Vector2(0.1, 0.5);
auto constraint = ExpressionEqualityConstraint<Vector2>(g, Vector2::Zero(), sigmas);
EXPECT(constraint.noiseModel()->isConstrained());
EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
// Create 2 sets of values for testing.
Values values1, values2;
values1.insert(x1_key, Vector2(1, 1));
values1.insert(x2_key, Vector2(-1, -1));
values2.insert(x1_key, Vector2(1, 1));
values2.insert(x2_key, Vector2(1, 1));
// Check that values1 are feasible.
EXPECT(constraint.feasible(values1));
// Check that violation evaluates as 0 at values1.
auto expected_violation1 = (Vector(2) << 0, 0).finished();
EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
// Check that values2 are indeed deemed infeasible.
EXPECT(!constraint.feasible(values2));
// Check constraint violation is indeed g(x) at values2.
auto expected_violation2 = (Vector(2) << 2, 2).finished();
EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
// Check scaled violation is indeed g(x)/sigmas at values2.
auto expected_scaled_violation2 = (Vector(2) << 20, 4).finished();
EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
// Check dim is the dimension of the vector.
EXPECT(constraint.dim() == 2);
// Generate factor representing the term in merit function.
double mu = 4;
auto merit_factor = constraint.penaltyFactor(mu);
// Check that noise model sigma == sigmas/sqrt(mu).
auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
// Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
double expected_error1 = 0; // 0.5 * 4 * ||[1, 0.5]||_([0.1,0.5]^2)^2
EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
double expected_error2 = 832; // 0.5 * 4 * ||[2, 2]||_([0.1,0.5]^2)^2
EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
// Check jacobian computation is correct.
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
}
// Test methods of FactorZeroErrorConstraint.
TEST(ZeroCostConstraint, BetweenFactor) {
Key x1_key = 1;
Key x2_key = 2;
Vector sigmas = Vector2(0.5, 0.1);
auto noise = noiseModel::Diagonal::Sigmas(sigmas);
auto factor = std::make_shared<BetweenFactor<Vector2>>(x1_key, x2_key, Vector2(1, 1), noise);
auto constraint = ZeroCostConstraint(factor);
EXPECT(constraint.noiseModel()->isConstrained());
EXPECT(assert_equal(sigmas, constraint.noiseModel()->sigmas()));
// Create 2 sets of values for testing.
Values values1, values2;
values1.insert(x1_key, Vector2(1, 1));
values1.insert(x2_key, Vector2(2, 2));
values2.insert(x1_key, Vector2(0, 0));
values2.insert(x2_key, Vector2(2, 3));
// Check that values1 are feasible.
EXPECT(constraint.feasible(values1));
// Check that violation evaluates as 0 at values1.
auto expected_violation1 = (Vector(2) << 0, 0).finished();
EXPECT(assert_equal(expected_violation1, constraint.unwhitenedError(values1)));
auto expected_scaled_violation1 = (Vector(2) << 0, 0).finished();
EXPECT(assert_equal(expected_scaled_violation1, constraint.whitenedError(values1)));
// Check that values2 are indeed deemed infeasible.
EXPECT(!constraint.feasible(values2));
// Check constraint violation is indeed g(x) at values2.
auto expected_violation2 = (Vector(2) << 1, 2).finished();
EXPECT(assert_equal(expected_violation2, constraint.unwhitenedError(values2)));
// Check scaled violation is indeed g(x)/sigmas at values2.
auto expected_scaled_violation2 = (Vector(2) << 2, 20).finished();
EXPECT(assert_equal(expected_scaled_violation2, constraint.whitenedError(values2)));
// Check dim is the dimension of the vector.
EXPECT(constraint.dim() == 2);
// Generate factor representing the term in merit function.
double mu = 4;
auto merit_factor = constraint.penaltyFactor(mu);
// Check that noise model sigma == sigmas/sqrt(mu).
auto expected_noise = noiseModel::Diagonal::Sigmas(sigmas / sqrt(mu));
EXPECT(expected_noise->equals(*merit_factor->noiseModel()));
// Check that error is equal to 0.5*mu*||g(x)+bias)||^2_Diag(sigmas^2).
double expected_error1 = 0; // 0.5 * 4 * ||[0, 0]||_([0.5,0.1]^2)^2
EXPECT(assert_equal(expected_error1, merit_factor->error(values1)));
double expected_error2 = 808; // 0.5 * 4 * ||[1, 2]||_([0.5,0.1]^2)^2
EXPECT(assert_equal(expected_error2, merit_factor->error(values2)));
// Check jacobian computation is correct.
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values1, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(*merit_factor, values2, 1e-7, 1e-5);
}
TEST(NonlinearEqualityConstraints, Container) {
NonlinearEqualityConstraints constraints;
Vector sigmas1 = Vector1(10);
auto g1 = x1 + pow(x1, 3) + x2 + pow(x2, 2);
Vector2_ x1_vec_expr(x1_key);
Vector2_ x2_vec_expr(x2_key);
auto g2 = x1_vec_expr + x2_vec_expr;
Vector sigmas2 = Vector2(0.1, 0.5);
constraints.emplace_shared<ExpressionEqualityConstraint<double>>(g1, 0.0, sigmas1);
constraints.emplace_shared<ExpressionEqualityConstraint<Vector2>>(g2, Vector2::Zero(), sigmas2);
// Check size.
EXPECT_LONGS_EQUAL(2, constraints.size());
// Check dimension.
EXPECT_LONGS_EQUAL(3, constraints.dim());
// Check keys.
KeySet expected_keys;
expected_keys.insert(x1_key);
expected_keys.insert(x2_key);
EXPECT(assert_container_equality(expected_keys, constraints.keys()));
// Check VariableIndex.
VariableIndex vi(constraints);
FactorIndices expected_vi_x1{0, 1};
FactorIndices expected_vi_x2{0, 1};
EXPECT(assert_container_equality(expected_vi_x1, vi[x1_key]));
EXPECT(assert_container_equality(expected_vi_x2, vi[x2_key]));
// Check constraint violation.
}
TEST(NonlinearEqualityConstraints, FromCostGraph) {}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

117
gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp Normal file
View File

@ -0,0 +1,117 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testNonlinearInequalityConstraint.h
* @brief unit tests for nonlinear inequality constraints
* @author Yetong Zhang
* @date Aug 3, 2024
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
#include <gtsam/nonlinear/factorTesting.h>
#include "constrainedExample.h"
using namespace gtsam;
using constrained_example::pow;
using constrained_example::x1, constrained_example::x2;
using constrained_example::x1_key, constrained_example::x2_key;
// Test methods of DoubleExpressionEquality.
TEST(NonlinearInequalityConstraint, ScalarExpressionInequalityConstraint) {
// create constraint from double expression
// g(x1, x2) = x1 + x1^3 + x2 + x2^2, from Vanderbergh slides
double sigma = 0.1;
auto g = x1 + pow(x1, 3) + x2 + pow(x2, 2);
auto constraint_geq = ScalarExpressionInequalityConstraint::GeqZero(g, sigma);
auto constraint_leq = ScalarExpressionInequalityConstraint::LeqZero(g, sigma);
// Check dimension is 1 for scalar g.
EXPECT_LONGS_EQUAL(1, constraint_geq->dim());
EXPECT_LONGS_EQUAL(1, constraint_leq->dim());
// Check keys include x1, x2.
KeyVector expected_keys{x1_key, x2_key};
EXPECT(assert_container_equality(expected_keys, constraint_leq->keys()));
// Create 3 sets of values for testing.
Values values1, values2, values3;
values1.insert(x1_key, -1.0);
values1.insert(x2_key, 1.0);
values2.insert(x1_key, 1.0);
values2.insert(x2_key, 1.0);
values3.insert(x1_key, -2.0);
values3.insert(x2_key, 1.0);
// Check that expression evaluation at different values.
EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedExpr(values1)));
EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedExpr(values2)));
EXPECT(assert_equal(Vector1(-8.0), constraint_leq->unwhitenedExpr(values3)));
EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedExpr(values1)));
EXPECT(assert_equal(Vector1(-4.0), constraint_geq->unwhitenedExpr(values2)));
EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedExpr(values3)));
// Check that constraint violation at different values.
EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values1)));
EXPECT(assert_equal(Vector1(4.0), constraint_leq->unwhitenedError(values2)));
EXPECT(assert_equal(Vector1(0.0), constraint_leq->unwhitenedError(values3)));
EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values1)));
EXPECT(assert_equal(Vector1(0.0), constraint_geq->unwhitenedError(values2)));
EXPECT(assert_equal(Vector1(8.0), constraint_geq->unwhitenedError(values3)));
// Check feasible.
EXPECT(constraint_leq->feasible(values1));
EXPECT(!constraint_leq->feasible(values2));
EXPECT(constraint_leq->feasible(values3));
EXPECT(constraint_geq->feasible(values1));
EXPECT(constraint_geq->feasible(values2));
EXPECT(!constraint_geq->feasible(values3));
// Check active.
EXPECT(constraint_leq->active(values1));
EXPECT(constraint_leq->active(values2));
EXPECT(!constraint_leq->active(values3));
EXPECT(constraint_geq->active(values1));
EXPECT(!constraint_geq->active(values2));
EXPECT(constraint_geq->active(values3));
// Check whitenedError of penalty factor.
// Expected to be sqrt(mu) / sigma * ramp(g(x))
double mu = 9.0;
auto penalty_leq = constraint_leq->penaltyFactor(mu);
auto penalty_geq = constraint_geq->penaltyFactor(mu);
EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values1)));
EXPECT(assert_equal(Vector1(120.0), penalty_leq->whitenedError(values2)));
EXPECT(assert_equal(Vector1(0.0), penalty_leq->whitenedError(values3)));
EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values1)));
EXPECT(assert_equal(Vector1(0.0), penalty_geq->whitenedError(values2)));
EXPECT(assert_equal(Vector1(240.0), penalty_geq->whitenedError(values3)));
// Check create equality constraint
auto constraint_eq1 = constraint_leq->createEqualityConstraint();
auto constraint_eq2 = constraint_geq->createEqualityConstraint();
EXPECT(assert_equal(0.0, constraint_eq1->error(values1)));
EXPECT(assert_equal(800.0, constraint_eq1->error(values2)));
EXPECT(assert_equal(3200.0, constraint_eq1->error(values3)));
EXPECT(assert_equal(0.0, constraint_eq2->error(values1)));
EXPECT(assert_equal(800.0, constraint_eq2->error(values2)));
EXPECT(assert_equal(3200.0, constraint_eq2->error(values3)));
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

76
gtsam/geometry/Pose3.cpp
View File

@ -183,18 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
// Get angular velocity omega and translational velocity v from twist xi
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
// Compute translation and optionally its Jacobian Q in w
// The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
#else
const Rot3 R(local.expmap());
#endif
// The translation t = local.leftJacobian() * v.
// Here we call applyLeftJacobian, which is faster if you don't need
// Jacobians, and returns Jacobian of t with respect to w if asked.
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
// t_parallel = w * w.dot(v); // translation parallel to axis
// w_cross_v = w.cross(v); // translation orthogonal to axis
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
// but functor does not need R, deals automatically with the case where theta2
// is near zero, and also gives us the machinery for the Jacobians.
Matrix3 H;
const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr);
if (Hxi) {
*Hxi << Jr, Z_3x3, //
Q, Jr;
// The Jacobian of expmap is given by the right Jacobian of SO(3):
const Matrix3 Jr = local.rightJacobian();
// We are creating a Pose3, so we still need to chain H with R^T, the
// Jacobian of Pose3::Create with respect to t.
const Matrix3 Q = R.matrix().transpose() * H;
*Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap
Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v.
}
return Pose3(R, t);
@ -260,45 +279,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
Matrix3 Q;
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
}
/* ************************************************************************* */
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
// t_parallel = w * w.dot(v); // translation parallel to axis
// w_cross_v = w.cross(v); // translation orthogonal to axis
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
// but functor does not need R, deals automatically with the case where theta2
// is near zero, and also gives us the machinery for the Jacobians.
Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q,
OptionalJacobian<3, 3> J,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);
// Instantiate functor for Dexp-related operations:
bool nearZero = (w.dot(w) <= nearZeroThreshold);
so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
// don't need Jacobians, and returns Jacobian of t with respect to w if asked.
// Call applyLeftJacobian to get its Jacobian
Matrix3 H;
Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
local.applyLeftJacobian(v, H);
// We return Jacobians for use in Expmap, so we multiply with X, that
// translates from left to right for our right expmap convention:
if (Q) {
Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
*Q = X * H;
}
if (J) {
*J = local.rightJacobian(); // = X * local.leftJacobian();
}
return t;
// Multiply with R^T to account for the Pose3::Create Jacobian.
const Matrix3 R = local.expmap();
return R.transpose() * H;
}
/* ************************************************************************* */

17
gtsam/geometry/Pose3.h
View File

@ -220,27 +220,10 @@ public:
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
* This is the Jacobian of ExpmapTranslation and computed there.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
/**
* Compute the translation part of the exponential map, with Jacobians.
* @param w 3D angular velocity
* @param v 3D velocity
* @param Q Optionally, compute 3x3 Jacobian wrpt w
* @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
* @param nearZeroThreshold threshold for small values
* @note This function returns Jacobians Q and J corresponding to the bottom
* blocks of the SE(3) exponential, and translated from left to right from the
* applyLeftJacobian Jacobians.
*/
static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q = {},
OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

12
gtsam/geometry/SO3.cpp
View File

@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
init(nearZeroApprox);
}
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
template <>
GTSAM_EXPORT
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
return so3::ExpmapFunctor(axis, theta).expmap();
return SO3(so3::ExpmapFunctor(axis, theta).expmap());
}
//******************************************************************************
@ -251,11 +251,11 @@ template <>
GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
so3::DexpFunctor local(omega);
*H = local.dexp();
return SO3(local.expmap());
} else {
return so3::ExpmapFunctor(omega).expmap();
return SO3(so3::ExpmapFunctor(omega).expmap());
}
}

2
gtsam/geometry/SO3.h
View File

@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor {
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
Matrix3 expmap() const;
protected:
void init(bool nearZeroApprox = false);

20
gtsam/geometry/tests/testSO3.cpp
View File

@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) {
// axis angle version
so3::ExpmapFunctor f1(axis, angle);
SO3 actual1 = f1.expmap();
SO3 actual1(f1.expmap());
CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
// axis angle version, negative angle
so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
SO3 actual2 = f2.expmap();
SO3 actual2(f2.expmap());
CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
// omega version
so3::ExpmapFunctor f3(axis * angle);
SO3 actual3 = f3.expmap();
SO3 actual3(f3.expmap());
CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
// omega version, negative angle
so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
SO3 actual4 = f4.expmap();
SO3 actual4(f4.expmap());
CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
}
@ -333,7 +333,7 @@ TEST(SO3, CrossB) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).crossB(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
@ -351,7 +351,7 @@ TEST(SO3, DoubleCrossC) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
@ -369,7 +369,7 @@ TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyDexp(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
@ -390,7 +390,7 @@ TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
@ -412,7 +412,7 @@ TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
@ -433,7 +433,7 @@ TEST(SO3, ApplyLeftJacobianInverse) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {

11
gtsam/inference/BayesTree-inst.h
View File

@ -196,21 +196,20 @@ namespace gtsam {
for (auto&& root: roots_) {
std::queue<sharedClique> bfs_queue;
// first, move the root to the queue
bfs_queue.push(root);
root = nullptr; // now the root node is owned by the queue
// first, steal the root and move it to the queue. This invalidates root
bfs_queue.push(std::move(root));
// do a BFS on the tree, for each node, add its children to the queue, and then delete it from the queue
// So if the reference count of the node is 1, it will be deleted, and because its children are in the queue,
// the deletion of the node will not trigger a recursive deletion of the children.
while (!bfs_queue.empty()) {
// move the ownership of the front node from the queue to the current variable
auto current = bfs_queue.front();
// move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue
auto current = std::move(bfs_queue.front());
bfs_queue.pop();
// add the children of the current node to the queue, so that the queue will also own the children nodes.
for (auto child: current->children) {
bfs_queue.push(child);
bfs_queue.push(std::move(child));
} // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
// the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
// deletion of the children.

12
gtsam/inference/ClusterTree-inst.h
View File

@ -114,20 +114,20 @@ ClusterTree<GRAPH>::~ClusterTree() {
for (auto&& root : roots_) {
std::queue<sharedNode> bfs_queue;
// first, move the root to the queue
bfs_queue.push(root);
root = nullptr; // now the root node is owned by the queue
// first, steal the root and move it to the queue. This invalidates root
bfs_queue.push(std::move(root));
// for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue.
// so that the recursive deletion will not happen.
while (!bfs_queue.empty()) {
// move the ownership of the front node from the queue to the current variable
auto node = bfs_queue.front();
// move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue
auto node = std::move(bfs_queue.front());
bfs_queue.pop();
// add the children of the current node to the queue, so that the queue will also own the children nodes.
for (auto child : node->children) {
bfs_queue.push(child);
bfs_queue.push(std::move(child));
} // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
// the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
// deletion of the children.

2
gtsam/inference/ClusterTree.h
View File

@ -166,9 +166,9 @@ class ClusterTree {
/// @}
protected:
~ClusterTree();
protected:
/// @name Details
/// @{

11
gtsam/inference/EliminationTree-inst.h
View File

@ -197,20 +197,19 @@ namespace gtsam {
for (auto&& root : roots_) {
std::queue<sharedNode> bfs_queue;
// first, move the root to the queue
bfs_queue.push(root);
root = nullptr; // now the root node is owned by the queue
// first, steal the root and move it to the queue. This invalidates root
bfs_queue.push(std::move(root));
// for each node iterated, if its reference count is 1, it will be deleted while its children are still in the queue.
// so that the recursive deletion will not happen.
while (!bfs_queue.empty()) {
// move the ownership of the front node from the queue to the current variable
auto node = bfs_queue.front();
// move the ownership of the front node from the queue to the current variable, invalidating the sharedClique at the front of the queue
auto node = std::move(bfs_queue.front());
bfs_queue.pop();
// add the children of the current node to the queue, so that the queue will also own the children nodes.
for (auto&& child : node->children) {
bfs_queue.push(child);
bfs_queue.push(std::move(child));
} // leaving the scope of current will decrease the reference count of the current node by 1, and if the reference count is 0,
// the node will be deleted. Because the children are in the queue, the deletion of the node will not trigger a recursive
// deletion of the children.

4
gtsam/inference/EliminationTree.h
View File

@ -117,8 +117,10 @@ namespace gtsam {
/// @}
public:
protected:
~EliminationTree();
public:
/// @name Standard Interface
/// @{

125
gtsam/navigation/GPSFactor.cpp
View File

@ -64,6 +64,69 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
return make_pair(nTb, nV);
}
//***************************************************************************
void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << bL_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactorArm::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(bL_, e->bL_, tol);
}
//***************************************************************************
Vector GPSFactorArm::evaluateError(const Pose3& p,
OptionalMatrixType H) const {
const Matrix3 nRb = p.rotation().matrix();
if (H) {
H->resize(3, 6);
H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
H->block<3, 3>(0, 3) = nRb;
}
return p.translation() + nRb * bL_ - nT_;
}
//***************************************************************************
void GPSFactorArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArmCalib on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactorArmCalib::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
Vector GPSFactorArmCalib::evaluateError(const Pose3& p, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.rotation().matrix();
if (H1) {
H1->resize(3, 6);
H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
H1->block<3, 3>(0, 3) = nRb;
}
if (H2){
H2->resize(3, 3);
*H2 = nRb;
}
return p.translation() + nRb * bL - nT_;
}
//***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
@ -85,5 +148,67 @@ Vector GPSFactor2::evaluateError(const NavState& p,
}
//***************************************************************************
void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2Arm on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << bL_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactor2Arm::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(bL_, e->bL_, tol);
}
//***************************************************************************
Vector GPSFactor2Arm::evaluateError(const NavState& p,
OptionalMatrixType H) const {
const Matrix3 nRb = p.attitude().matrix();
if (H) {
H->resize(3, 9);
H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
H->block<3, 3>(0, 3) = nRb;
H->block<3, 3>(0, 6).setZero();
}
return p.position() + nRb * bL_ - nT_;
}
//***************************************************************************
void GPSFactor2ArmCalib::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2ArmCalib on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool GPSFactor2ArmCalib::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
}
//***************************************************************************
Vector GPSFactor2ArmCalib::evaluateError(const NavState& p, const Point3& bL,
OptionalMatrixType H1, OptionalMatrixType H2) const {
const Matrix3 nRb = p.attitude().matrix();
if (H1) {
H1->resize(3, 9);
H1->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL);
H1->block<3, 3>(0, 3) = nRb;
H1->block<3, 3>(0, 6).setZero();
}
if (H2){
H2->resize(3, 3);
*H2 = nRb;
}
return p.position() + nRb * bL - nT_;
}
}/// namespace gtsam

291
gtsam/navigation/GPSFactor.h
View File

@ -25,6 +25,8 @@ namespace gtsam {
/**
* Prior on position in a Cartesian frame.
* If there exists a non-zero lever arm between body frame and GPS
* antenna, instead use GPSFactorArm.
* Possibilities include:
* ENU: East-North-Up navigation frame at some local origin
* NED: North-East-Down navigation frame at some local origin
@ -38,7 +40,7 @@ private:
typedef NoiseModelFactorN<Pose3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
public:
@ -83,6 +85,7 @@ public:
/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
@ -112,7 +115,146 @@ private:
};
/**
* Version of GPSFactor for NavState
* Version of GPSFactor (for Pose3) with lever arm between GPS and Body frame.
* Because the actual location of the antenna depends on both position and
* attitude, providing a lever arm makes components of the attitude observable
* and accounts for position measurement vs state discrepancies while turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {
private:
typedef NoiseModelFactorN<Pose3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
///< position of the GPS antenna in the body frame
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactorArm> shared_ptr;
/// Typedef to this class
typedef GPSFactorArm This;
/// default constructor - only use for serialization
GPSFactorArm():nT_(0, 0, 0), bL_(0, 0, 0) {}
~GPSFactorArm() override {}
/** Constructor from a measurement in a Cartesian frame.
* @param key key of the Pose3 variable related to this measurement
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
* @param model Gaussian noise model
*/
GPSFactorArm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn), bL_(leverArm) {
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
/// return the lever arm, a position in the body frame
inline const Point3 & leverArm() const {
return bL_;
}
};
/**
* Version of GPSFactorArm (for Pose3) with unknown lever arm between GPS and
* Body frame. Because the actual location of the antenna depends on both
* position and attitude, providing a lever arm makes components of the attitude
* observable and accounts for position measurement vs state discrepancies while
* turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactorArmCalib: public NoiseModelFactorN<Pose3, Point3> {
private:
typedef NoiseModelFactorN<Pose3, Point3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactorArmCalib> shared_ptr;
/// Typedef to this class
typedef GPSFactorArmCalib This;
/// default constructor - only use for serialization
GPSFactorArmCalib() : nT_(0, 0, 0) {}
~GPSFactorArmCalib() override {}
/** Constructor from a measurement in a Cartesian frame.
* @param key1 key of the Pose3 variable related to this measurement
* @param key2 key of the Point3 variable related to the lever arm
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
* @param model Gaussian noise model
*/
GPSFactorArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key1, key2), nT_(gpsIn) {
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const Pose3& p, const Point3& bL, OptionalMatrixType H1,
OptionalMatrixType H2) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
};
/**
* Version of GPSFactor for NavState, assuming zero lever arm between body frame
* and GPS. If there exists a non-zero lever arm between body frame and GPS
* antenna, instead use GPSFactor2Arm.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
@ -121,7 +263,7 @@ private:
typedef NoiseModelFactorN<NavState> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
public:
@ -139,7 +281,11 @@ public:
~GPSFactor2() override {}
/// Constructor from a measurement in a Cartesian frame.
/** Constructor from a measurement in a Cartesian frame.
* @param key key of the NavState variable related to this measurement
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param model Gaussian noise model
*/
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn) {
}
@ -160,6 +306,7 @@ public:
/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
@ -180,4 +327,140 @@ private:
#endif
};
/**
* Version of GPSFactor2 with lever arm between GPS and Body frame.
* Because the actual location of the antenna depends on both position and
* attitude, providing a lever arm makes components of the attitude observable
* and accounts for position measurement vs state discrepancies while turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {
private:
typedef NoiseModelFactorN<NavState> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
///< position of the GPS antenna in the body frame
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactor2Arm> shared_ptr;
/// Typedef to this class
typedef GPSFactor2Arm This;
/// default constructor - only use for serialization
GPSFactor2Arm():nT_(0, 0, 0), bL_(0, 0, 0) {}
~GPSFactor2Arm() override {}
/** Constructor from a measurement in a Cartesian frame.
* @param key key of the NavState variable related to this measurement
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param leverArm translation from the body frame origin to the gps antenna, in body frame
* @param model noise model for the factor's residual
*/
GPSFactor2Arm(Key key, const Point3& gpsIn, const Point3& leverArm, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn), bL_(leverArm) {
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
/// return the lever arm, a position in the body frame
inline const Point3 & leverArm() const {
return bL_;
}
};
/**
* Version of GPSFactor2Arm for an unknown lever arm between GPS and Body frame.
* Because the actual location of the antenna depends on both position and
* attitude, providing a lever arm makes components of the attitude observable
* and accounts for position measurement vs state discrepancies while turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2ArmCalib: public NoiseModelFactorN<NavState, Point3> {
private:
typedef NoiseModelFactorN<NavState, Point3> Base;
Point3 nT_; ///< Position measurement in cartesian coordinates (navigation frame)
public:
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactor2ArmCalib> shared_ptr;
/// Typedef to this class
typedef GPSFactor2ArmCalib This;
/// default constructor - only use for serialization
GPSFactor2ArmCalib():nT_(0, 0, 0) {}
~GPSFactor2ArmCalib() override {}
/** Constructor from a measurement in a Cartesian frame.
* @param key1 key of the NavState variable related to this measurement
* @param key2 key of the Point3 variable related to the lever arm
* @param gpsIn gps measurement, in Cartesian navigation frame
* @param model Gaussian noise model
*/
GPSFactor2ArmCalib(Key key1, Key key2, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key1, key2), nT_(gpsIn) {
}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/// print
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
/// vector of errors
Vector evaluateError(const NavState& p, const Point3& bL,
OptionalMatrixType H1,
OptionalMatrixType H2) const override;
/// return the measurement, in the navigation frame
inline const Point3 & measurementIn() const {
return nT_;
}
};
} /// namespace gtsam

56
gtsam/navigation/NavState.cpp
View File

@ -117,20 +117,32 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
// Get angular velocity w and components rho (for t) and nu (for v) from xi
Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
// Compute translations and optionally their Jacobians Q in w
// The Jacobians with respect to rho and nu are equal to Jr
Matrix3 Qt, Qv;
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
#else
const Rot3 R(local.expmap());
#endif
// Compute translation and velocity. See Pose3::Expmap
Matrix3 H_t_w, H_v_w;
const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr);
const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);
if (Hxi) {
*Hxi << Jr, Z_3x3, Z_3x3, //
Qt, Jr, Z_3x3, //
Qv, Z_3x3, Jr;
const Matrix3 Jr = local.rightJacobian();
// We are creating a NavState, so we still need to chain H_t_w and H_v_w
// with R^T, the Jacobian of Navstate::Create with respect to both t and v.
const Matrix3 M = R.matrix();
*Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap
M.transpose() * H_t_w, Jr, Z_3x3, //
M.transpose() * H_v_w, Z_3x3, Jr;
// In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83).
// J_l is the Jacobian of applyLeftJacobian in the second argument.
}
return NavState(R, t, v);
@ -231,11 +243,22 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt);
Pose3::ExpmapTranslation(w, nu, Qv);
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w, H_v_w;
local.applyLeftJacobian(rho, H_t_w);
local.applyLeftJacobian(nu, H_v_w);
// Multiply with R^T to account for NavState::Create Jacobian.
const Matrix3 R = local.expmap();
const Matrix3 Qt = R.transpose() * H_t_w;
const Matrix3 Qv = R.transpose() * H_v_w;
// Now compute the blocks of the LogmapDerivative Jacobian
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw;
@ -247,7 +270,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
ChartJacobian Hxi) {

2
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -144,7 +144,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
auto p = testing::Params();
testing::SomeMeasurements measurements;
auto preintegrated = [=](const Vector3& a, const Vector3& w) {
auto preintegrated = [&](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(p, Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();

148
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -44,7 +44,11 @@ const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;
const double lat = 33.87071, lon = -84.30482, h = 274;\
// Random lever arm
const Point3 leverArm(0.1, 0.2, 0.3);
}
// *************************************************************************
@ -79,6 +83,77 @@ TEST( GPSFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
// *************************************************************************
TEST( GPSFactorArm, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactorArm factor(key, Point3(E, N, U), leverArm, model);
// Create a linearization point at zero error
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 np = Point3(E, N, U) - nRb * leverArm;
Pose3 T(nRb, np);
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
[&factor](const Pose3& T) { return factor.evaluateError(T); }, T);
// Use the factor to calculate the derivative
Matrix actualH;
factor.evaluateError(T, actualH);
// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
// *************************************************************************
TEST( GPSFactorArmCalib, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key1(1), key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactorArmCalib factor(key1, key2, Point3(E, N, U), model);
// Create a linearization point at zero error
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 np = Point3(E, N, U) - nRb * leverArm;
Pose3 T(nRb, np);
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
[&factor, &leverArm](const Pose3& pose_arg) {
return factor.evaluateError(pose_arg, leverArm);
},
T);
Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
[&factor, &T](const Point3& point_arg) {
return factor.evaluateError(T, point_arg);
},
leverArm);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(T, leverArm, actualH1, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
// *************************************************************************
TEST( GPSFactor2, Constructor ) {
using namespace example;
@ -108,6 +183,77 @@ TEST( GPSFactor2, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
// *************************************************************************
TEST( GPSFactor2Arm, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2Arm factor(key, Point3(E, N, U), leverArm, model);
// Create a linearization point at zero error
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 np = Point3(E, N, U) - nRb * leverArm;
NavState T(nRb, np, Vector3::Zero());
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, NavState>(
[&factor](const NavState& T) { return factor.evaluateError(T); }, T);
// Use the factor to calculate the derivative
Matrix actualH;
factor.evaluateError(T, actualH);
// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
// *************************************************************************
TEST( GPSFactor2ArmCalib, Constructor ) {
using namespace example;
// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);
// Factor
Key key1(1), key2(2);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2ArmCalib factor(key1, key2, Point3(E, N, U), model);
// Create a linearization point at zero error
const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 np = Point3(E, N, U) - nRb * leverArm;
NavState T(nRb, np, Vector3::Zero());
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T, leverArm),1e-5));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector, NavState>(
[&factor, &leverArm](const NavState& nav_arg) {
return factor.evaluateError(nav_arg, leverArm);
},
T);
Matrix expectedH2 = numericalDerivative11<Vector, Point3>(
[&factor, &T](const Point3& point_arg) {
return factor.evaluateError(T, point_arg);
},
leverArm);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2;
factor.evaluateError(T, leverArm, actualH1, actualH2);
// Verify we get the expected error
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
}
//***************************************************************************
TEST(GPSData, init) {

4
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -399,7 +399,7 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
Vector3 measuredOmega(0.1, 0, 0);
double deltaT = 0.5;
auto evaluateRotation = [=](const Vector3 biasOmega) {
auto evaluateRotation = [&measuredOmega, &deltaT](const Vector3 biasOmega) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
};
@ -424,7 +424,7 @@ TEST(ImuFactor, PartialDerivativeLogmap) {
// Measurements
Vector3 deltaTheta(0, 0, 0);
auto evaluateLogRotation = [=](const Vector3 delta) {
auto evaluateLogRotation = [&thetaHat](const Vector3 delta) {
return Rot3::Logmap(
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
};

6
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -43,21 +43,21 @@ TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
std::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
[&](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
std::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
[&](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
std::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
[=](const Vector3& a, const Vector3& w) {
[&](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaVij();

4
gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -78,7 +78,7 @@ TEST(ImuFactor, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
std::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
[&](const Vector3& a, const Vector3& w) {
TangentPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
@ -149,7 +149,7 @@ TEST(TangentPreintegration, Compose) {
TEST(TangentPreintegration, MergedBiasDerivatives) {
testing::SomeMeasurements measurements;
auto f = [=](const Vector3& a, const Vector3& w) {
auto f = [&](const Vector3& a, const Vector3& w) {
TangentPreintegration pim02(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim02);
testing::integrateMeasurements(measurements, &pim02);

4
gtsam/nonlinear/Expression.h
View File

@ -34,6 +34,8 @@ namespace gtsam {
// Forward declares
class Values;
template<typename T> class ExpressionFactor;
template<typename T> class ExpressionEqualityConstraint;
class ScalarExpressionInequalityConstraint;
namespace internal {
template<typename T> class ExecutionTrace;
@ -206,6 +208,8 @@ protected:
// be very selective on who can access these private methods:
friend class ExpressionFactor<T> ;
friend class internal::ExpressionNode<T>;
friend class ExpressionEqualityConstraint<T>;
friend class ScalarExpressionInequalityConstraint;
// and add tests
friend class ::ExpressionFactorShallowTest;

74
gtsam/nonlinear/NonlinearEquality.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
@ -40,14 +41,11 @@ namespace gtsam {
* \nosubgrouping
*/
template<class VALUE>
class NonlinearEquality: public NoiseModelFactorN<VALUE> {
class NonlinearEquality: public NonlinearEqualityConstraint {
public:
typedef VALUE T;
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
private:
// feasible value
@ -63,7 +61,7 @@ private:
using This = NonlinearEquality<VALUE>;
// typedef to base class
using Base = NoiseModelFactorN<VALUE>;
using Base = NonlinearEqualityConstraint;
public:
@ -88,7 +86,7 @@ public:
const CompareFunction &_compare = std::bind(traits<T>::Equals,
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
KeyVector{j}), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
compare_(_compare) {
}
@ -99,10 +97,14 @@ public:
const CompareFunction &_compare = std::bind(traits<T>::Equals,
std::placeholders::_1, std::placeholders::_2, 1e-9)) :
Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
KeyVector{j}), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
compare_(_compare) {
}
Key key() const {
return keys().front();
}
/// @}
/// @name Testable
/// @{
@ -139,7 +141,7 @@ public:
}
/// Error function
Vector evaluateError(const T& xj, OptionalMatrixType H) const override {
Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const {
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
@ -158,11 +160,20 @@ public:
}
}
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
VALUE x1 = x.at<VALUE>(key());
if (H) {
return evaluateError(x1, &(H->front()));
} else {
return evaluateError(x1);
}
}
/// Linearize is over-written, because base linearization tries to whiten
GaussianFactor::shared_ptr linearize(const Values& x) const override {
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
Vector b = evaluateError(xj, &A);
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(
new JacobianFactor(this->key(), A, b, model));
@ -206,16 +217,13 @@ struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUE>
class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
class NonlinearEquality1: public NonlinearEqualityConstraint {
public:
typedef VALUE X;
// Provide access to Matrix& version of evaluateError:
using NoiseModelFactor1<VALUE>::evaluateError;
protected:
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearEqualityConstraint Base;
typedef NonlinearEquality1<VALUE> This;
/// Default constructor to allow for serialization
@ -240,7 +248,7 @@ public:
NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
: Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
std::abs(mu)),
key),
KeyVector{key}),
value_(value) {}
~NonlinearEquality1() override {
@ -252,14 +260,25 @@ public:
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
Key key() const { return keys().front(); }
/// g(x) with optional derivative
Vector evaluateError(const X& x1, OptionalMatrixType H) const override {
Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const {
if (H)
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
return traits<X>::Local(value_,x1);
}
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
X x1 = x.at<X>(key());
if (H) {
return evaluateError(x1, &(H->front()));
} else {
return evaluateError(x1);
}
}
/// Print
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
@ -298,10 +317,10 @@ struct traits<NonlinearEquality1<VALUE> >
* be the same.
*/
template <class T>
class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
class NonlinearEquality2 : public NonlinearEqualityConstraint {
protected:
using Base = NoiseModelFactorN<T, T>;
using This = NonlinearEquality2<T>;
typedef NonlinearEqualityConstraint Base;
typedef NonlinearEquality2<T> This;
GTSAM_CONCEPT_MANIFOLD_TYPE(T)
@ -311,9 +330,6 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
public:
typedef std::shared_ptr<NonlinearEquality2<T>> shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
/**
* Constructor
@ -323,7 +339,7 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
*/
NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
: Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
key1, key2) {}
KeyVector{key1, key2}) {}
~NonlinearEquality2() override {}
/// @return a deep copy of this factor
@ -334,13 +350,23 @@ class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
/// g(x) with optional derivative2
Vector evaluateError(
const T& x1, const T& x2, OptionalMatrixType H1, OptionalMatrixType H2) const override {
const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const {
static const size_t p = traits<T>::dimension;
if (H1) *H1 = -Matrix::Identity(p, p);
if (H2) *H2 = Matrix::Identity(p, p);
return traits<T>::Local(x1, x2);
}
Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
T x1 = x.at<T>(keys().front());
T x2 = x.at<T>(keys().back());
if (H) {
return evaluateError(x1, x2, &(H->front()), &(H->back()));
} else {
return evaluateError(x1, x2);
}
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
private:

2
gtsam/nonlinear/NonlinearFactor.h
View File

@ -281,6 +281,8 @@ public:
*/
double weight(const Values& c) const;
using NonlinearFactor::error;
/**
* Calculate the error of the factor.
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.

65
gtsam/slam/BoundingConstraint.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
namespace gtsam {
@ -30,20 +31,17 @@ namespace gtsam {
* @ingroup slam
*/
template<class VALUE>
struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
struct BoundingConstraint1: public NonlinearInequalityConstraint {
typedef VALUE X;
typedef NoiseModelFactorN<VALUE> Base;
typedef NonlinearInequalityConstraint Base;
typedef std::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint1(Key key, double threshold,
bool isGreaterThan, double mu = 1000.0) :
Base(noiseModel::Constrained::All(1, mu), key),
Base(noiseModel::Constrained::All(1, mu), KeyVector{key}),
threshold_(threshold), isGreaterThan_(isGreaterThan) {
}
@ -51,6 +49,7 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
inline double threshold() const { return threshold_; }
inline bool isGreaterThan() const { return isGreaterThan_; }
inline Key key() const { return keys().front(); }
/**
* function producing a scalar value to compare to the threshold
@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
virtual double value(const X& x, OptionalMatrixType H =
OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X>(this->key()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override {
if (H) {
double d = value(x.at<X>(this->key()), &(H->front()));
if (isGreaterThan_) {
H->front() *= -1;
return Vector1(threshold_ - d);
} else {
return Vector1(d - threshold_);
}
} else {
double d = value(x.at<X>(this->key()));
return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_);
}
}
Vector evaluateError(const X& x, OptionalMatrixType H) const override {
/// TODO: This should be deprecated.
Vector evaluateError(const X& x, OptionalMatrixType H = {}) const {
Matrix D;
double error = value(x, &D) - threshold_;
if (H) {
@ -102,22 +110,19 @@ private:
* to implement for specific systems
*/
template<class VALUE1, class VALUE2>
struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
struct BoundingConstraint2: public NonlinearInequalityConstraint {
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
typedef NonlinearInequalityConstraint Base;
typedef std::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;
double threshold_;
bool isGreaterThan_; /// flag for greater/less than
BoundingConstraint2(Key key1, Key key2, double threshold,
bool isGreaterThan, double mu = 1000.0)
: Base(noiseModel::Constrained::All(1, mu), key1, key2),
: Base(noiseModel::Constrained::All(1, mu), KeyVector{key1, key2}),
threshold_(threshold), isGreaterThan_(isGreaterThan) {}
~BoundingConstraint2() override {}
@ -133,15 +138,27 @@ struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone) const = 0;
/** active when constraint *NOT* met */
bool active(const Values& c) const override {
// note: still active at equality to avoid zigzagging
double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override {
X1 x1 = x.at<X1>(keys().front());
X2 x2 = x.at<X2>(keys().back());
if (H) {
double d = value(x1, x2, &(H->front()), &(H->back()));
if (isGreaterThan_) {
H->front() *= -1;
H->back() *= -1;
return Vector1(threshold_ - d);
} else {
return Vector1(d - threshold_);
}
} else {
double d = value(x1, x2);
return Vector1((isGreaterThan_) ? threshold_ - d : d - threshold_);
}
}
/// TODO: This should be deprecated.
Vector evaluateError(const X1& x1, const X2& x2,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const {
Matrix D1, D2;
double error = value(x1, x2, &D1, &D2) - threshold_;
if (H1) {

3
gtsam_unstable/discrete/examples/schedulingExample.cpp
View File

@ -108,7 +108,8 @@ void runLargeExample() {
// Do brute force product and output that to file
if (scheduler.nrStudents() == 1) { // otherwise too slow
DecisionTreeFactor product = scheduler.product();
DecisionTreeFactor product =
*std::dynamic_pointer_cast<DecisionTreeFactor>(scheduler.product());
product.dot("scheduling-large", DefaultKeyFormatter, false);
}

3
gtsam_unstable/discrete/examples/schedulingQuals12.cpp
View File

@ -108,7 +108,8 @@ void runLargeExample() {
// Do brute force product and output that to file
if (scheduler.nrStudents() == 1) { // otherwise too slow
DecisionTreeFactor product = scheduler.product();
DecisionTreeFactor product =
*std::dynamic_pointer_cast<DecisionTreeFactor>(scheduler.product());
product.dot("scheduling-large", DefaultKeyFormatter, false);
}

3
gtsam_unstable/discrete/examples/schedulingQuals13.cpp
View File

@ -132,7 +132,8 @@ void runLargeExample() {
// Do brute force product and output that to file
if (scheduler.nrStudents() == 1) { // otherwise too slow
DecisionTreeFactor product = scheduler.product();
DecisionTreeFactor product =
*std::dynamic_pointer_cast<DecisionTreeFactor>(scheduler.product());
product.dot("scheduling-large", DefaultKeyFormatter, false);
}

265
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -16,29 +16,37 @@
* @date May 23, 2012
*/
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/base/debug.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/debug.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h> // For writeG2o
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <string>
using namespace std;
using namespace gtsam;
Key MakeKey(size_t index) { return Symbol('x', index); }
using symbol_shorthand::X;
using BetweenPoint2 = BetweenFactor<Point2>;
/* ************************************************************************* */
bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) {
bool check_smoother(const NonlinearFactorGraph& fullgraph,
const Values& fullinit,
const IncrementalFixedLagSmoother& smoother,
const Key& key) {
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
VectorValues delta = linearized.optimize();
Values fullfinal = fullinit.retract(delta);
@ -50,51 +58,46 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini
}
/* ************************************************************************* */
void PrintSymbolicTreeHelper(
const ISAM2Clique::shared_ptr& clique, const std::string indent = "") {
void PrintSymbolicTreeHelper(const ISAM2Clique::shared_ptr& clique,
const std::string indent = "") {
// Print the current clique
std::cout << indent << "P( ";
for(Key key: clique->conditional()->frontals()) {
for (Key key : clique->conditional()->frontals()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
if (clique->conditional()->nrParents() > 0)
std::cout << "| ";
for(Key key: clique->conditional()->parents()) {
if (clique->conditional()->nrParents() > 0) std::cout << "| ";
for (Key key : clique->conditional()->parents()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
// Recursively print all of the children
for(const ISAM2Clique::shared_ptr& child: clique->children) {
for (const ISAM2Clique::shared_ptr& child : clique->children) {
PrintSymbolicTreeHelper(child, indent + " ");
}
}
/* ************************************************************************* */
void PrintSymbolicTree(const ISAM2& isam,
const std::string& label) {
void PrintSymbolicTree(const ISAM2& isam, const std::string& label) {
std::cout << label << std::endl;
if (!isam.roots().empty()) {
for(const ISAM2::sharedClique& root: isam.roots()) {
for (const ISAM2::sharedClique& root : isam.roots()) {
PrintSymbolicTreeHelper(root);
}
} else
std::cout << "{Empty Tree}" << std::endl;
}
/* ************************************************************************* */
TEST( IncrementalFixedLagSmoother, Example )
{
// Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and
// the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at
// the end of the smoothing lag)
TEST(IncrementalFixedLagSmoother, Example) {
// Test the IncrementalFixedLagSmoother in a pure linear environment. Thus,
// full optimization and the IncrementalFixedLagSmoother should be identical
// (even with the linearized approximations at the end of the smoothing lag)
SETDEBUG("IncrementalFixedLagSmoother update", true);
// Set up parameters
SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
// Create a Fixed-Lag Smoother
@ -110,13 +113,13 @@ TEST( IncrementalFixedLagSmoother, Example )
// Add a prior at time 0 and update the HMF
{
Key key0 = MakeKey(0);
Key key0 = X(0);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise);
newFactors.addPrior(key0, Point2(0.0, 0.0), odoNoise);
newValues.insert(key0, Point2(0.01, 0.01));
newTimestamps[key0] = 0.0;
@ -133,16 +136,17 @@ TEST( IncrementalFixedLagSmoother, Example )
}
// Add odometry from time 0 to time 5
while(i <= 5) {
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
while (i <= 5) {
Key key1 = X(i - 1);
Key key2 = X(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newFactors.emplace_shared<BetweenPoint2>(key1, key2, Point2(1.0, 0.0),
odoNoise);
newValues.insert(key2, Point2(double(i) + 0.1, -0.1));
newTimestamps[key2] = double(i);
fullgraph.push_back(newFactors);
@ -157,19 +161,22 @@ TEST( IncrementalFixedLagSmoother, Example )
++i;
}
// Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM
// Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to
// the TSM
{
// Add the odometry factor to the HMF
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
Key key1 = X(i - 1);
Key key2 = X(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.push_back(BetweenFactor<Point2>(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newFactors.emplace_shared<BetweenPoint2>(key1, key2, Point2(1.0, 0.0),
odoNoise);
newFactors.emplace_shared<BetweenPoint2>(X(2), X(5), Point2(3.5, 0.0),
loopNoise);
newValues.insert(key2, Point2(double(i) + 0.1, -0.1));
newTimestamps[key2] = double(i);
fullgraph.push_back(newFactors);
@ -185,19 +192,21 @@ TEST( IncrementalFixedLagSmoother, Example )
}
// Add odometry from time 6 to time 15
while(i <= 15) {
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
while (i <= 15) {
Key key1 = X(i - 1);
Key key2 = X(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
// Add the odometry factor twice to ensure the removeFactor test below works,
// where we need to keep the connectivity of the graph.
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
// Add the odometry factor twice to ensure the removeFactor test below
// works, where we need to keep the connectivity of the graph.
newFactors.emplace_shared<BetweenPoint2>(key1, key2, Point2(1.0, 0.0),
odoNoise);
newFactors.emplace_shared<BetweenPoint2>(key1, key2, Point2(1.0, 0.0),
odoNoise);
newValues.insert(key2, Point2(double(i) + 0.1, -0.1));
newTimestamps[key2] = double(i);
fullgraph.push_back(newFactors);
@ -214,112 +223,120 @@ TEST( IncrementalFixedLagSmoother, Example )
// add/remove an extra factor
{
Key key1 = MakeKey(i-1);
Key key2 = MakeKey(i);
Key key1 = X(i - 1);
Key key2 = X(i);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
// add 2 odometry factors
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newValues.insert(key2, Point2(double(i)+0.1, -0.1));
newTimestamps[key2] = double(i);
++i;
// add 2 odometry factors
newFactors.emplace_shared<BetweenPoint2>(key1, key2, Point2(1.0, 0.0),
odoNoise);
newFactors.emplace_shared<BetweenPoint2>(key1, key2, Point2(1.0, 0.0),
odoNoise);
newValues.insert(key2, Point2(double(i) + 0.1, -0.1));
newTimestamps[key2] = double(i);
++i;
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
fullgraph.push_back(newFactors);
fullinit.insert(newValues);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Update the smoother
smoother.update(newFactors, newValues, newTimestamps);
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
// now remove one of the two and try again
// empty values and new factors for fake update in which we only remove factors
NonlinearFactorGraph emptyNewFactors;
Values emptyNewValues;
Timestamps emptyNewTimestamps;
// now remove one of the two and try again
// empty values and new factors for fake update in which we only remove
// factors
NonlinearFactorGraph emptyNewFactors;
Values emptyNewValues;
Timestamps emptyNewTimestamps;
size_t factorIndex = 25; // any index that does not break connectivity of the graph
FactorIndices factorToRemove;
factorToRemove.push_back(factorIndex);
size_t factorIndex =
25; // any index that does not break connectivity of the graph
FactorIndices factorToRemove;
factorToRemove.push_back(factorIndex);
const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors();
const NonlinearFactorGraph smootherFactorsBeforeRemove =
smoother.getFactors();
std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl;
std::cout << "smootherFactorsBeforeRemove.size() = "
<< smootherFactorsBeforeRemove.size() << std::endl;
// remove factor
smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove);
// remove factor
smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,
factorToRemove);
// Note: the following test (checking that the number of factor is reduced by 1)
// fails since we are not reusing slots, hence also when removing a factor we do not change
// the size of the factor graph
// size_t nrFactorsAfterRemoval = smoother.getFactors().size();
// DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5);
// Note: the following test (checking that the number of factor is reduced
// by 1) fails since we are not reusing slots, hence also when removing a
// factor we do not change the size of the factor graph size_t
// nrFactorsAfterRemoval = smoother.getFactors().size();
// DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5);
// check that the factors in the smoother are right
NonlinearFactorGraph actual = smoother.getFactors();
for(size_t i=0; i< smootherFactorsBeforeRemove.size(); i++){
// check that the factors that were not removed are there
if(smootherFactorsBeforeRemove[i] && i != factorIndex){
EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i]));
}
else{ // while the factors that were not there or were removed are no longer there
EXPECT(!actual[i]);
}
}
// check that the factors in the smoother are right
NonlinearFactorGraph actual = smoother.getFactors();
for (size_t i = 0; i < smootherFactorsBeforeRemove.size(); i++) {
// check that the factors that were not removed are there
if (smootherFactorsBeforeRemove[i] && i != factorIndex) {
EXPECT(smootherFactorsBeforeRemove[i]->equals(*actual[i]));
} else { // while the factors that were not there or were removed are no
// longer there
EXPECT(!actual[i]);
}
}
}
{
SETDEBUG("BayesTreeMarginalizationHelper", true);
PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:");
PrintSymbolicTree(smoother.getISAM2(),
"Bayes Tree Before marginalization test:");
// Do pressure test on marginalization. Enlarge max_i to enhance the test.
const int max_i = 500;
while(i <= max_i) {
Key key_0 = MakeKey(i);
Key key_1 = MakeKey(i-1);
Key key_2 = MakeKey(i-2);
Key key_3 = MakeKey(i-3);
Key key_4 = MakeKey(i-4);
Key key_5 = MakeKey(i-5);
Key key_6 = MakeKey(i-6);
Key key_7 = MakeKey(i-7);
Key key_8 = MakeKey(i-8);
Key key_9 = MakeKey(i-9);
Key key_10 = MakeKey(i-10);
while (i <= max_i) {
Key key_0 = X(i);
Key key_1 = X(i - 1);
Key key_2 = X(i - 2);
Key key_3 = X(i - 3);
Key key_4 = X(i - 4);
Key key_5 = X(i - 5);
Key key_6 = X(i - 6);
Key key_7 = X(i - 7);
Key key_8 = X(i - 8);
Key key_9 = X(i - 9);
Key key_10 = X(i - 10);
NonlinearFactorGraph newFactors;
Values newValues;
Timestamps newTimestamps;
// To make a complex graph
newFactors.push_back(BetweenFactor<Point2>(key_1, key_0, Point2(1.0, 0.0), odometerNoise));
const Point2 z(1.0, 0.0);
newFactors.emplace_shared<BetweenPoint2>(key_1, key_0, z, odoNoise);
if (i % 2 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_2, key_1, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_2, key_1, z, odoNoise);
if (i % 3 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_3, key_2, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_3, key_2, z, odoNoise);
if (i % 4 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_4, key_3, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_4, key_3, z, odoNoise);
if (i % 5 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_5, key_4, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_5, key_4, z, odoNoise);
if (i % 6 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_6, key_5, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_6, key_5, z, odoNoise);
if (i % 7 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_7, key_6, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_7, key_6, z, odoNoise);
if (i % 8 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_8, key_7, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_8, key_7, z, odoNoise);
if (i % 9 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_9, key_8, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_9, key_8, z, odoNoise);
if (i % 10 == 0)
newFactors.push_back(BetweenFactor<Point2>(key_10, key_9, Point2(1.0, 0.0), odometerNoise));
newFactors.emplace_shared<BetweenPoint2>(key_10, key_9, z, odoNoise);
newValues.insert(key_0, Point2(double(i)+0.1, -0.1));
newValues.insert(key_0, Point2(double(i) + 0.1, -0.1));
newTimestamps[key_0] = double(i);
fullgraph.push_back(newFactors);
@ -330,13 +347,17 @@ TEST( IncrementalFixedLagSmoother, Example )
// Check
CHECK(check_smoother(fullgraph, fullinit, smoother, key_0));
PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i));
PrintSymbolicTree(
smoother.getISAM2(),
"Bayes Tree marginalization test: i = " + std::to_string(i));
++i;
}
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

14
tests/testBoundingConstraint.cpp
View File

@ -90,8 +90,8 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
EXPECT(constraint2.active(config));
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
}
@ -103,10 +103,10 @@ TEST( testBoundingConstraint, unary_basics_active2 ) {
config.insert(key, pt1);
EXPECT(constraint3.active(config));
EXPECT(constraint4.active(config));
EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(-1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(-1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
// EXPECT(assert_equal(-1.0 * I_1x1, constraint3.evaluateError(pt1), tol));
// EXPECT(assert_equal(-1.0 * I_1x1, constraint4.evaluateError(pt1), tol));
EXPECT(assert_equal(1.0 * I_1x1, constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(1.0 * I_1x1, constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(5.0, constraint3.error(config), tol);
EXPECT_DOUBLES_EQUAL(5.0, constraint4.error(config), tol);
}
@ -213,7 +213,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
config1.update(key2, pt4);
EXPECT(rangeBound.active(config1));
EXPECT(assert_equal(-1.0*I_1x1, rangeBound.unwhitenedError(config1)));
EXPECT(assert_equal(1.0*I_1x1, rangeBound.unwhitenedError(config1)));
EXPECT_DOUBLES_EQUAL(0.5*mu, rangeBound.error(config1), tol);
}