libsurvive
libsurvive copied to clipboard
cntools
Prevent libsurvive from continuously optimizing the calibration.
Hello,
is there an option from preventing libsurvive from changing the calibration file continuously. I want to deploy libsurvive tracking on multiple robots individually but noticed that the library keeps "optimizing" the individual lighthouse poses ever so slightly. This naturally leads to the coordinate systems of each robot shifting over time.
I already attempted to disable calibration with --disable-calibrate or setting "PositionSet": "1" for each lighthouse inside the calibration JSON (even though I have no clue what PositionSet does. I also tried just making the calibration file read-only but that doesn't stop libsurvive from loading the contents and simply changing the calibration in memory.
Does the environment variable SURVIVE_GLOBALSCENESOLVER=0 do what you want?
Hello,
thank you for your quick reply. Unfortunately it didn't help, the calibration is still being changed:
Ouput
sfg-agent@sfg-experimental-area:/workspaces/colcon_ws/src/sfg_lighthouse_tracking/config$ git restore lighthouse_calibration.json
sfg-agent@sfg-experimental-area:/workspaces/colcon_ws/src/sfg_lighthouse_tracking/config$ export SURVIVE_GLOBALSCENESOLVER=0
sfg-agent@sfg-experimental-area:/workspaces/colcon_ws/src/sfg_lighthouse_tracking/config$ ros2 launch sfg_lighthouse_tracking lighthouse_tracker.launch.py
[INFO] [launch]: All log files can be found below /home/sfg-agent/.ros/log/2025-07-04-14-31-01-349973-sfg-experimental-area-15700
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [lighthouse_tracker-1]: process started with pid [15701]
[lighthouse_tracker-1] [INFO] [1751632261.391753909] [lighthouse_tracker]: Loading device to frame id mapping with mappings:
[lighthouse_tracker-1] [INFO] [1751632261.391857389] [lighthouse_tracker]: LHB-77E57906 -> lighthouse_west
[lighthouse_tracker-1] [INFO] [1751632261.391871501] [lighthouse_tracker]: LHB-9AC77B3F -> lighthouse_east
[lighthouse_tracker-1] [INFO] [1751632261.391876359] [lighthouse_tracker]: LHB-BFEC7A60 -> lighthouse_north
[lighthouse_tracker-1] [INFO] [1751632261.391880385] [lighthouse_tracker]: LHB-F87476E -> lighthouse_south
[lighthouse_tracker-1] [INFO] [1751632261.391884311] [lighthouse_tracker]: LHR-A0F109D9 -> sfg_tb4_04
[lighthouse_tracker-1] [INFO] [1751632261.391888176] [lighthouse_tracker]: LHR-A0FD9A68 -> sfg_go2_01
[lighthouse_tracker-1] [INFO] [1751632261.391902599] [lighthouse_tracker]: Loading libsurvive context with parameters:
[lighthouse_tracker-1] [INFO] [1751632261.391924753] [lighthouse_tracker]: --configfile /workspaces/colcon_ws/install/sfg_lighthouse_tracking/share/sfg_lighthouse_tracking/config/lighthouse_calibration.json
[lighthouse_tracker-1] [INFO] [1751632261.391930362] [lighthouse_tracker]: --disable-calibrate 1
[lighthouse_tracker-1] [INFO] [1751632261.391934178] [lighthouse_tracker]: --init-config /workspaces/colcon_ws/install/sfg_lighthouse_tracking/share/sfg_lighthouse_tracking/config/lighthouse_calibration.json
[lighthouse_tracker-1] [INFO] [1751632261.391938355] [lighthouse_tracker]: --lighthouse-gen 2
[lighthouse_tracker-1] [INFO] [1751632261.391943813] [lighthouse_tracker]: --lighthousecount 4
[lighthouse_tracker-1] Info: Loaded drivers: HTCVive
[lighthouse_tracker-1] [INFO] [1751632261.409978575] [lighthouse_tracker]: Started lighthouse tracker.
[lighthouse_tracker-1] [INFO] [1751632261.410059130] [lighthouse_tracker]: Event processing thread for libsurvive started.
[lighthouse_tracker-1] Info: Adding tracked object WM0 from HTC
[lighthouse_tracker-1] [INFO] [1751632261.434151631] [lighthouse_tracker]: Received device added event for object: lighthouse_south
[lighthouse_tracker-1] [INFO] [1751632261.534264930] [lighthouse_tracker]: Received device added event for object: lighthouse_north
[lighthouse_tracker-1] [INFO] [1751632261.634392632] [lighthouse_tracker]: Received device added event for object: lighthouse_east
[lighthouse_tracker-1] [INFO] [1751632261.734533083] [lighthouse_tracker]: Received device added event for object: lighthouse_west
[lighthouse_tracker-1] [INFO] [1751632261.834694116] [lighthouse_tracker]: Received device added event for object:
[lighthouse_tracker-1] Info: Device WM0 has watchman FW version 1592875850 and FPGA version 538/7/2; named ' watchman'. Hardware id 0x84020109 Board rev: 3 (len 56)
[lighthouse_tracker-1] [INFO] [1751632262.259934940] [lighthouse_tracker]: Received config event for object: sfg_tb4_04, config: {
[lighthouse_tracker-1] "device_class": "generic_tracker",
[lighthouse_tracker-1] "device_pid": 8960,
[lighthouse_tracker-1] "device_serial_number": "LHR-A0F109D9",
[lighthouse_tracker-1] "device_vid": 10462,
[lighthouse_tracker-1] "firmware_config": {
[lighthouse_tracker-1] "button_function_5": "PA2",
[lighthouse_tracker-1] "button_function_6": "PA2",
[lighthouse_tracker-1] "bwd_config": "0x0499",
[lighthouse_tracker-1] "charge_current_low_temp_ma": 204,
[lighthouse_tracker-1] "charge_current_ma": 512,
[lighthouse_tracker-1] "charge_current_nominal_ac_ma": 768,
[lighthouse_tracker-1] "charge_current_nominal_usb_ma": 512,
[lighthouse_tracker-1] "charge_term_current_ma": 128,
[lighthouse_tracker-1] "charge_voltage_limit_mv": 4400,
[lighthouse_tracker-1] "charging_low_temp_threshold_c": 15,
[lighthouse_tracker-1] "haptic_pulse_width_multiplier": "1000",
[lighthouse_tracker-1] "haptics": true,
[lighthouse_tracker-1] "mode": "controller",
[lighthouse_tracker-1] "radio": true,
[lighthouse_tracker-1] "recharge_voltage_mv": 100,
[lighthouse_tracker-1] "spi_flash": false,
[lighthouse_tracker-1] "sys_min_voltage_mv": 3500,
[lighthouse_tracker-1] "trackpad": false,
[lighthouse_tracker-1] "trigger": false,
[lighthouse_tracker-1] "vrc": true
[lighthouse_tracker-1] },
[lighthouse_tracker-1] "head": {
[lighthouse_tracker-1] "plus_x": [
[lighthouse_tracker-1] 1,
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "plus_z": [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] -1
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "position": [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] -0.001500000013038516
[lighthouse_tracker-1] ]
[lighthouse_tracker-1] },
[lighthouse_tracker-1] "htcComposeTime": "2024-06-18 13:23:32.484000",
[lighthouse_tracker-1] "imu": {
[lighthouse_tracker-1] "acc_bias": [
[lighthouse_tracker-1] 0.098,
[lighthouse_tracker-1] 0.09475,
[lighthouse_tracker-1] -0.2651
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "acc_scale": [
[lighthouse_tracker-1] 1.001,
[lighthouse_tracker-1] 1,
[lighthouse_tracker-1] 0.9952
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "gyro_bias": [
[lighthouse_tracker-1] -0.008258,
[lighthouse_tracker-1] 0.009,
[lighthouse_tracker-1] -0.02022
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "gyro_scale": [
[lighthouse_tracker-1] 1.0,
[lighthouse_tracker-1] 1.0,
[lighthouse_tracker-1] 1.0
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "plus_x": [
[lighthouse_tracker-1] 3.422854177870249e-08,
[lighthouse_tracker-1] 0.9999999657714582,
[lighthouse_tracker-1] 0
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "plus_z": [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 1
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "position": [
[lighthouse_tracker-1] 0.013760336674749851,
[lighthouse_tracker-1] -0.0031355370301753283,
[lighthouse_tracker-1] 0.006249960046261549
[lighthouse_tracker-1] ]
[lighthouse_tracker-1] },
[lighthouse_tracker-1] "lighthouse_config": {
[lighthouse_tracker-1] "channelMap": [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 1,
[lighthouse_tracker-1] 2,
[lighthouse_tracker-1] 3,
[lighthouse_tracker-1] 4,
[lighthouse_tracker-1] 5,
[lighthouse_tracker-1] 6,
[lighthouse_tracker-1] 7,
[lighthouse_tracker-1] 8,
[lighthouse_tracker-1] 9,
[lighthouse_tracker-1] 10,
[lighthouse_tracker-1] 11,
[lighthouse_tracker-1] 12,
[lighthouse_tracker-1] 13,
[lighthouse_tracker-1] 14,
[lighthouse_tracker-1] 15,
[lighthouse_tracker-1] 16,
[lighthouse_tracker-1] 17,
[lighthouse_tracker-1] 18,
[lighthouse_tracker-1] 19
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "modelNormals": [
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.6226087808609009,
[lighthouse_tracker-1] 0.7406917810440063,
[lighthouse_tracker-1] 0.2524559199810028
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.5794840455055237,
[lighthouse_tracker-1] -0.33456504344940186,
[lighthouse_tracker-1] 0.7431450486183167
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.8324770927429199,
[lighthouse_tracker-1] 0.4806310534477234,
[lighthouse_tracker-1] 0.27563703060150146
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.3186790943145752,
[lighthouse_tracker-1] -0.9125622510910034,
[lighthouse_tracker-1] 0.25626906752586365
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.8595702052116394,
[lighthouse_tracker-1] 0.49627313017845154,
[lighthouse_tracker-1] -0.1218690276145935
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.8595702052116394,
[lighthouse_tracker-1] -0.49627313017845154,
[lighthouse_tracker-1] -0.1218690276145935
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.945646345615387,
[lighthouse_tracker-1] 0.2721450924873352,
[lighthouse_tracker-1] 0.17801706492900848
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] -0.9848077893257141,
[lighthouse_tracker-1] 0.1736479550600052
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0.6691307425498962,
[lighthouse_tracker-1] 0.7431447505950928
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0.4694717526435852,
[lighthouse_tracker-1] 0.882947564125061
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0.22495102882385254,
[lighthouse_tracker-1] 0.9743701219558716
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] 0.9925462603569031,
[lighthouse_tracker-1] -0.1218690276145935
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.9409955143928528,
[lighthouse_tracker-1] 0.2890631854534149,
[lighthouse_tracker-1] 0.17598310112953186
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0,
[lighthouse_tracker-1] -0.9925462603569031,
[lighthouse_tracker-1] -0.1218690276145935
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.8595702052116394,
[lighthouse_tracker-1] -0.49627313017845154,
[lighthouse_tracker-1] -0.1218690276145935
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.3186790943145752,
[lighthouse_tracker-1] -0.9125622510910034,
[lighthouse_tracker-1] 0.25626906752586365
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.8595702052116394,
[lighthouse_tracker-1] 0.49627313017845154,
[lighthouse_tracker-1] -0.1218690276145935
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.8324770927429199,
[lighthouse_tracker-1] 0.4806310534477234,
[lighthouse_tracker-1] 0.27563703060150146
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.5794840455055237,
[lighthouse_tracker-1] -0.33456504344940186,
[lighthouse_tracker-1] 0.7431450486183167
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.6226087808609009,
[lighthouse_tracker-1] 0.7406917810440063,
[lighthouse_tracker-1] 0.2524559199810028
[lighthouse_tracker-1] ]
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] "modelPoints": [
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.02295893058180809,
[lighthouse_tracker-1] 0.025090694427490234,
[lighthouse_tracker-1] 0.03349452465772629
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.0252121239900589,
[lighthouse_tracker-1] 0.015219956636428833,
[lighthouse_tracker-1] 0.03769969940185547
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.034530434757471085,
[lighthouse_tracker-1] 0.020328262820839882,
[lighthouse_tracker-1] 0.028880523517727852
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.032777730375528336,
[lighthouse_tracker-1] 0.007793750148266554,
[lighthouse_tracker-1] 0.03357292339205742
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.02840486541390419,
[lighthouse_tracker-1] 0.017061907798051834,
[lighthouse_tracker-1] 0.004910310730338097
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.023524697870016098,
[lighthouse_tracker-1] -0.012651534751057625,
[lighthouse_tracker-1] 0.004790011793375015
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.010048427619040012,
[lighthouse_tracker-1] -0.0312424898147583,
[lighthouse_tracker-1] 0.03381083160638809
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.0003703251131810248,
[lighthouse_tracker-1] -0.03953124210238457,
[lighthouse_tracker-1] 0.029550395905971527
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.00011690279643516988,
[lighthouse_tracker-1] -0.028139416128396988,
[lighthouse_tracker-1] 0.03769994527101517
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.0002383288083365187,
[lighthouse_tracker-1] -0.017211267724633217,
[lighthouse_tracker-1] 0.020176712423563004
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 4.9654805479804054e-05,
[lighthouse_tracker-1] 0.022511692717671394,
[lighthouse_tracker-1] 0.015089163556694984
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 9.800370025914162e-05,
[lighthouse_tracker-1] 0.027681762352585793,
[lighthouse_tracker-1] 0.004754286725074053
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.01029757596552372,
[lighthouse_tracker-1] -0.03149173781275749,
[lighthouse_tracker-1] 0.03358480706810951
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] -0.00017596417455933988,
[lighthouse_tracker-1] -0.03549216315150261,
[lighthouse_tracker-1] 0.012657178565859795
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.02297048643231392,
[lighthouse_tracker-1] -0.013131482526659966,
[lighthouse_tracker-1] 0.004630872048437595
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.032580289989709854,
[lighthouse_tracker-1] 0.007858114317059517,
[lighthouse_tracker-1] 0.03386056423187256
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.02847636491060257,
[lighthouse_tracker-1] 0.016725247725844383,
[lighthouse_tracker-1] 0.004746856167912483
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.03417275846004486,
[lighthouse_tracker-1] 0.02037651278078556,
[lighthouse_tracker-1] 0.028860967606306076
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.025340218096971512,
[lighthouse_tracker-1] 0.015178924426436424,
[lighthouse_tracker-1] 0.03796328604221344
[lighthouse_tracker-1] ],
[lighthouse_tracker-1] [
[lighthouse_tracker-1] 0.02290763519704342,
[lighthouse_tracker-1] 0.025226574391126633,
[lighthouse_tracker-1] 0.033814687281847
[lighthouse_tracker-1] ]
[lighthouse_tracker-1] ]
[lighthouse_tracker-1] },
[lighthouse_tracker-1] "manufacturer": "HTC",
[lighthouse_tracker-1] "model_number": "VIVE Tracker 3.0 MV",
[lighthouse_tracker-1] "render_model": "{htc}vr_tracker_vive_3_0",
[lighthouse_tracker-1] "resource_root": "htc",
[lighthouse_tracker-1] "revision": 3,
[lighthouse_tracker-1] "tracked_controller_role": "",
[lighthouse_tracker-1] "type": "Lighthouse_HMD"
[lighthouse_tracker-1] }
[lighthouse_tracker-1] Info: Detected LH gen 2 system.
[lighthouse_tracker-1] Info: LightcapMode (WM0) 1 -> 2 (ff)
[lighthouse_tracker-1] Info: OOTX not set for LH in channel 2; attaching ootx decoder using device WM0
[lighthouse_tracker-1] Info: OOTX not set for LH in channel 1; attaching ootx decoder using device WM0
[lighthouse_tracker-1] Info: OOTX not set for LH in channel 0; attaching ootx decoder using device WM0
[lighthouse_tracker-1] Info: OOTX not set for LH in channel 3; attaching ootx decoder using device WM0
[lighthouse_tracker-1] Info: Attempting to solve for 0 with 5/5 meas from device WM0
[lighthouse_tracker-1] Info: Assuming 1 with 9/9 meas from device WM0 as given
[lighthouse_tracker-1] Info: Assuming 2 with 6/6 meas from device WM0 as given
[lighthouse_tracker-1] Info: Attempting to solve for 3 with 8/9 meas from device WM0
[lighthouse_tracker-1] Info: Assuming object position of +3.999065e-01 +5.098165e-01 +4.546444e-02 -1.725657e-01 +4.678062e-01 -5.305648e-01 +6.854776e-01
[lighthouse_tracker-1] Info: Using LH 0 (0f87476e) as reference lighthouse
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[lighthouse_tracker-1] [INFO] [1751632266.134940883] [rclcpp]: signal_handler(signum=2)
[lighthouse_tracker-1] [INFO] [1751632266.137981922] [lighthouse_tracker]: Event processing thread for libsurvive shut down.
[lighthouse_tracker-1] Info: MPFIT stats for WM0:
[lighthouse_tracker-1] Info: seed runs 2 / 425
[lighthouse_tracker-1] Info: error failures 0
[INFO] [lighthouse_tracker-1]: process has finished cleanly [pid 15701]
sfg-agent@sfg-experimental-area:/workspaces/colcon_ws/src/sfg_lighthouse_tracking/config$ git status
On branch main
Your branch is up to date with 'origin/main'.
Changes not staged for commit:
(use "git add <file>..." to update what will be committed)
(use "git restore <file>..." to discard changes in working directory)
modified: lighthouse_calibration.json
no changes added to commit (use "git add" and/or "git commit -a")
I was also unable to find any reference in the code to SURVIVE_GLOBALSCENESOLVER, so I'm unsure where this mechanism is implemented.
When I've been using libsurvive that option prevented it from messing up the calibration but I never checked whether it never updates the config file with it.
There's some arcane macro magic that reads environment variables and applies them as config settings here https://github.com/collabora/libsurvive/blob/32cf62c52744fdc32003ef8169e8b81f6f31526b/src/survive.c#L325
Yeah, I just checked and the changes in position are quite large (24cm in one instance):
diff --git a/config/lighthouse_calibration.json b/config/lighthouse_calibration.json
index fbd1660..ccfdf66 100644
--- a/config/lighthouse_calibration.json
+++ b/config/lighthouse_calibration.json
@@ -8,8 +8,8 @@
"index":"0",
"id":"260523886",
"mode":"1",
-"pose":["0.000493459520","5.398861885071","2.215373277664","0.120629101992","0.110769331455","-0.572545826435","-0.803349256516"],
-"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"],
+"pose":["0.240459725261","5.423994541168","2.125160694122","0.122166467632","0.084696790637","-0.582821001710","-0.798887656562"],
+"variance":["0.000004530067","0.000002909254","0.000005936905","0.005678600166","0.001677540597","0.003943505231"],
"unlock_count":"0",
"accel":["1.000000000000","127.000000000000","41.000000000000"],
"fcalphase":["0.000000000000","-0.003702163696"],
@@ -20,7 +20,7 @@
"fcalogeephase":["2.201171875000","2.968750000000"],
"fcalogeemag":["0.187500000000","0.155395507812"],
"OOTXSet":"1",
-"PositionSet":"0"
+"PositionSet":"1"
}
"lighthouse1":{
"index":"1",
@@ -62,7 +62,7 @@
"index":"3",
"id":"2011527430",
"mode":"0",
-"pose":["3.982059240341","1.159700155258","2.235741376877","-0.501658856869","-0.339023172855","-0.447369277477","-0.658226728439"],
+"pose":["4.086276531219","1.113584280014","2.086863994598","-0.499750854795","-0.353418739213","-0.458857121113","-0.644045355785"],
"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"],
"unlock_count":"0",
"accel":["6.000000000000","127.000000000000","45.000000000000"],
@@ -74,5 +74,5 @@
"fcalogeephase":["1.610351562500","2.332031250000"],
"fcalogeemag":["-0.025344848633","-0.156372070312"],
"OOTXSet":"1",
-"PositionSet":"0"
+"PositionSet":"1"
}
I will definitely need to figure out a way to stop the calibration from being adjusted.
Okay, when I manually change the calibration file, and for each lighthouse set
"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"],"PositionSet":"1"
the calibration doesn't seem to be altered anymore. I suppose this is because the lighthouse poses are also continuously optimized via some Kalman-Filter and a variance of 0 mathematically boils down to new_lighthouse_pose = old_lighthouse_pose for any pose update that might occur!? While my short testing showed that the calibration remains unchanged, I'm not very confident in it not changing at some point. Perhaps someone with deeper insight into the codebase can confirm my rationale?
Turns out even with
"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"]"PositionSet":"1"
the calibration changes:
sfg-agent@sfg-experimental-area:/workspaces/colcon_ws/src/sfg_lighthouse_tracking/config$ git diff HEAD lighthouse_calibration.json
diff --git a/config/lighthouse_calibration.json b/config/lighthouse_calibration.json
index 87930cd..6f5809a 100644
--- a/config/lighthouse_calibration.json
+++ b/config/lighthouse_calibration.json
@@ -26,10 +26,10 @@
"index":"1",
"id":"3219946080",
"mode":"3",
-"pose":["1.210296034813","-3.974362850189","2.276101827621","-0.812102172308","-0.559627634049","-0.105146784656","-0.127479906723"],
-"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"],
-"unlock_count":"0",
-"accel":["0.000000000000","127.000000000000","44.000000000000"],
+"pose":["1.261757848585","-3.940053646958","2.304855577598","-0.815164605058","-0.554031534349","-0.113477489662","-0.125214156338"],
+"variance":["0.002015625199","0.014186505873","0.718914514365","0.585777028496","8.890837241811","4.000472692920"],
+"unlock_count":"1",
+"accel":["-1.000000000000","127.000000000000","44.000000000000"],
"fcalphase":["0.000000000000","-0.005111694336"],
"fcaltilt":["-0.043182373047","0.045684814453"],
"fcalcurve":["0.343994140625","0.379394531250"],
@@ -44,10 +44,10 @@
"index":"2",
"id":"2596764479",
"mode":"2",
-"pose":["-3.333345651627","-1.389783501625","2.253699779510","-0.660506059304","-0.470543466581","0.336674893584","0.478508733161"],
+"pose":["-3.301829157193","-1.326178184366","2.302469420437","-0.687538475064","-0.450900261826","0.349122645948","0.449547747495"],
"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"],
-"unlock_count":"0",
-"accel":["3.000000000000","127.000000000000","42.000000000000"],
+"unlock_count":"1",
+"accel":["2.000000000000","127.000000000000","42.000000000000"],
"fcalphase":["0.000000000000","-0.002889633179"],
"fcaltilt":["-0.042419433594","0.045440673828"],
"fcalcurve":["0.207153320312","0.440673828125"],
@@ -62,10 +62,10 @@
"index":"3",
"id":"2011527430",
"mode":"0",
-"pose":["3.982059240341","1.159700155258","2.235741376877","-0.501658852637","-0.339023169996","-0.447369273704","-0.658226722887"],
+"pose":["4.081496431484","1.397736478359","1.947500618217","-0.495755186311","-0.332482904780","-0.485133075105","-0.639005330740"],
"variance":["0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000","0.000000000000"],
-"unlock_count":"0",
-"accel":["7.000000000000","127.000000000000","45.000000000000"],
+"unlock_count":"1",
+"accel":["5.000000000000","127.000000000000","45.000000000000"],
"fcalphase":["0.000000000000","-0.007797241211"],
"fcaltilt":["-0.048431396484","0.044036865234"],
"fcalcurve":["0.398193359375","0.095947265625"],
Okay, I did some digging and noticed that the update of the lighthouse poses occurs when the OOTX data transmitted by the lighthouse changes in comparison to the OOTX data stored in the calibration file (even if calibration is disabled). According to nairol/LighthouseRedox, the OOTX payload mostly consists of static fields that are set during factory calibration.
But notably, there are 3 fields mentioned in the docs that do change when re-powering a lighthouse (I have not observed them changing within a single power-cycle):
accel.dir_xaccel.dir_yaccel.dir_z
accel.dir_* represents the orientation as measured by the lighthouse's internal accelerometer (I think) and is used downstream by libsurvive for correct tracking (I think). Theoretically, if the lighthouses are fixed and do not move (as is the case for me), accel.dir_* should also not change and stay constant. But during testing I saw that accel.dir_* as transmitted by the OOTX does change marginally, around ±2 for a single axis at a time. I attribute these fluctuations to noise in the direction measurement by the internal accelerometer.
Regardless, a change in accel.dir_* triggers an update of the lighthouse position (see https://github.com/collabora/libsurvive/blob/master/src/survive_process_gen2.c#L50-L56) which causes the calibration (specifically the pose and pose variance) to change.
To fix this, I introduced a new config parameter --ignore-ootx-accel-dir-changed that when set to 1 tells ootx_packet_clbk_d_gen2 to ignore changes in accel.dir_*received by the OOTX:
STATIC_CONFIG_ITEM(SERIALIZE_OOTX, "serialize-ootx", 'b', "Serialize out ootx", 0)
STATIC_CONFIG_ITEM(IGNORE_OOTX_ACCEL_DIR_CHANGED, "ignore-ootx-accel-dir-changed", 'b', "", 0)
static void ootx_packet_clbk_d_gen2(ootx_decoder_context *ct, ootx_packet *packet) {
SurviveContext *ctx = ((SurviveObject *)(ct->user))->ctx;
int id = ct->user1;
lighthouse_info_v15 v15;
init_lighthouse_info_v15(&v15, packet->data);
if (survive_configi(ctx, SERIALIZE_OOTX_TAG, SC_GET, 0) == 1) {
char filename[128];
snprintf(filename, 128, "LH%02d_%08x.ootx", v15.mode_current & 0x7F, (unsigned)v15.id);
FILE *f = fopen(filename, "w");
fwrite(packet->data, packet->length, 1, f);
fclose(f);
}
BaseStationData *b = &ctx->bsd[id];
b->OOTXChecked |= true;
FLT accel[3] = {v15.accel_dir[0], v15.accel_dir[1], v15.accel_dir[2]};
bool upChanged = norm3d(b->accel) != 0.0 && dist3d(b->accel, accel) > 1e-3;
bool doSave = b->BaseStationID != v15.id || b->OOTXSet == false;
if (survive_configi(ctx, IGNORE_OOTX_ACCEL_DIR_CHANGED_TAG, SC_GET, 0) == 0) {
if (upChanged) {
SV_VERBOSE(10, "OOTX up direction changed for %x (%f)", b->BaseStationID, norm3d(b->accel));
}
doSave = doSave || upChanged;
}
b->OOTXSet = 1;
if (doSave) {
// ...
}
}
A similar change is also applied to ootx_packet_cblk_d_gen1. But I couldn't test that change because I do not have a gen 1 lighthouse.
Anyways, if desired I can create a pull request with the changes. Alternatively to the new introduction of the --ignore-ootx-accel-dir-changed parameter, one could also reuse the --disable-calibrate parameter to achieve the same thing. Perhaps this would actually be a better approach!?