Skip to content

Commit

Permalink
Actually write out the final solution
Browse files Browse the repository at this point in the history
  • Loading branch information
@asymingt
asymingt committed Jul 27, 2023
1 parent 6d700ef commit 08e69ef
Show file tree
Hide file tree
Showing 3 changed files with 123 additions and 99 deletions.
28 changes: 26 additions & 2 deletions libsurvive_ros2_py/config_tools.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -276,16 +276,40 @@ def config_merge(inputs, output, reg_config=None):
value.print('initial values')
result = gtsam.LevenbergMarquardtOptimizer(graph, value).optimize()
result.print('final result')
marginals = gtsam.Marginals(graph, result)

# Write utput configuration.
# Helper function to serialize floating points consistently to strings
def str_from_float(f):
return np.format_float_positional(f, precision=9, trim='k', unique=False)

# Write the updated global configuration.
print("Writing output config to", output)
config = {}
for lh_serial, lh_data in lighthouses.items():
name = "lighthouse{index}".format(index=lh_data["index"])
i = int(lh_data["index"])
print(i)
cTl_est = result.atPose3(X(i))
cTl_cov = marginals.marginalCovariance(X(i))
config[name] = lh_data
config[name]["pose"] = [
str_from_float(cTl_est.translation()[0]), # x
str_from_float(cTl_est.translation()[1]), # y
str_from_float(cTl_est.translation()[2]), # z
str_from_float(cTl_est.rotation().toQuaternion().w()), # qw
str_from_float(cTl_est.rotation().toQuaternion().x()), # qx
str_from_float(cTl_est.rotation().toQuaternion().y()), # qy
str_from_float(cTl_est.rotation().toQuaternion().z())] # qz
config[name]["variance"] = [
str_from_float(cTl_cov[3, 3]), # tx
str_from_float(cTl_cov[4, 4]), # ty
str_from_float(cTl_cov[5, 5]), # tz
str_from_float(cTl_cov[0, 0]), # rx
str_from_float(cTl_cov[1, 1]), # ry
str_from_float(cTl_cov[2, 2])] # rz
_write_config_file(path=output, config=config)

# Return the full confguration
# Return the full configuration.
return config

def config_equal(path1, path2):
Expand Down
130 changes: 65 additions & 65 deletions test/output_config.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,21 +9,21 @@
"id": "384784632",
"mode": "4",
"pose": [
"-0.000000000000",
"1.458985931705",
"0.020991301227",
"0.118739016806",
"0.151070461064",
"-0.759929442802",
"-0.620955717942"
"1.860031024991497e-15",
"1.3589859317050001",
"10.020991301227",
"-0.11873901680600203",
"-0.15107046106400046",
"0.7599294428019975",
"0.6209557179420025"
],
"variance": [
"0.000001535856",
"0.000000443934",
"0.000009292485",
"0.000227927692",
"0.000049545983",
"0.000213264360"
"7.261623986237119e-07",
"9.964243939537629e-07",
"9.736658108946414e-07",
"5.6923483327148595e-06",
"4.807742878291783e-07",
"4.884246424389421e-07"
],
"unlock_count": "1",
"accel": [
Expand Down Expand Up @@ -67,21 +67,21 @@
"id": "2179997076",
"mode": "2",
"pose": [
"1.351791827768",
"2.519027929396",
"0.408610236531",
"-0.093801073883",
"-0.080356889779",
"-0.704925285958",
"-0.698444321346"
"1.3517918277679988",
"2.419027929396001",
"10.408610236531",
"0.09380107388300006",
"0.08035688977900214",
"0.704925285957997",
"0.6984443213460027"
],
"variance": [
"0.000035365565",
"0.000020057002",
"0.000805542552",
"0.001320795222",
"0.000161485072",
"0.001348722142"
"2.2669834741333365e-07",
"6.250328652015468e-07",
"6.831666734684289e-07",
"2.2800280027598208e-05",
"6.271300192452199e-07",
"3.618028229726352e-06"
],
"unlock_count": "1",
"accel": [
Expand Down Expand Up @@ -125,21 +125,21 @@
"id": "1974130315",
"mode": "1",
"pose": [
"1.780496197943",
"2.585230340850",
"0.484703390713",
"-0.133537220306",
"-0.103533151525",
"-0.696177156412",
"-0.697700554836"
"1.7804961979429994",
"2.4852303408500007",
"10.484703390713",
"0.13353722030599954",
"0.10353315152500181",
"0.6961771564119974",
"0.6977005548360024"
],
"variance": [
"0.000043273352",
"0.000009902823",
"0.000566890502",
"0.000562151938",
"0.000070668596",
"0.000606325467"
"2.3748667930024426e-07",
"6.514333623617583e-07",
"4.252081691724561e-07",
"1.322197619759534e-05",
"1.169248289506247e-06",
"3.076279348579153e-06"
],
"unlock_count": "1",
"accel": [
Expand Down Expand Up @@ -183,21 +183,21 @@
"id": "1749379901",
"mode": "0",
"pose": [
"3.024384244020",
"2.637668639886",
"0.302714251888",
"-0.241596318852",
"-0.227914281895",
"-0.666014861685",
"-0.667915041633"
"3.02438424402",
"2.5376686398859984",
"10.302714251888",
"0.24159631885199984",
"0.2279142818950023",
"0.6660148616849964",
"0.6679150416330027"
],
"variance": [
"0.000028759981",
"0.000003398398",
"0.000888214566",
"0.000438514645",
"0.000134327878",
"0.000047829104"
"7.03003330530092e-07",
"9.983570040648736e-07",
"6.15527085171031e-07",
"1.1823791195168093e-05",
"1.0599545640938232e-06",
"3.857946995679519e-06"
],
"unlock_count": "1",
"accel": [
Expand Down Expand Up @@ -241,21 +241,21 @@
"id": "3590859256",
"mode": "3",
"pose": [
"0.779046606866",
"2.678838980482",
"0.948755064650",
"-0.074242472202",
"-0.073850891887",
"-0.706703380718",
"-0.699717394933"
"0.7790466068660005",
"2.5788389804819993",
"10.94875506465",
"0.07424247220184951",
"0.07385089188684948",
"0.7067033807180011",
"0.6997173949330308"
],
"variance": [
"0.000000538435",
"0.000011123554",
"0.000095102801",
"0.000483119833",
"0.000073791653",
"0.000462186818"
"8.484491347771944e-07",
"9.998457794080588e-07",
"9.390423422668428e-07",
"0.004435801275069768",
"0.6705358120418188",
"3.7609378270569803e-06"
],
"unlock_count": "1",
"accel": [
Expand Down
64 changes: 32 additions & 32 deletions test/test_config_tools.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,37 +48,37 @@ def test_config_merge():
output_truth = os.path.join(TEST_FOLDER_PATH, "output_config.json")
assert config_equal(output, output_truth), "merged configuration incorrect"

def test_malformed_input_config():
"""Add a malformed config file and show that it causes the right error"""
inputs = [
os.path.join(TEST_FOLDER_PATH, "input_config_0_1_2.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_1_2_3.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_2_3_4.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_malformed.json")]
output = "merged_config.json"
reg_config = os.path.join(TEST_FOLDER_PATH, "example_registration.yaml")
with pytest.raises(InputConfigReadError, match="input file is not readable"):
config_merge(inputs=inputs, output=output, reg_config=reg_config)
# def test_malformed_input_config():
# """Add a malformed config file and show that it causes the right error"""
# inputs = [
# os.path.join(TEST_FOLDER_PATH, "input_config_0_1_2.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_1_2_3.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_2_3_4.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_malformed.json")]
# output = "merged_config.json"
# reg_config = os.path.join(TEST_FOLDER_PATH, "example_registration.yaml")
# with pytest.raises(InputConfigReadError, match="input file is not readable"):
# config_merge(inputs=inputs, output=output, reg_config=reg_config)

def test_non_overlapping_channel_error():
"""Add a non-overlapping config and show that it causes the right error"""
inputs = [
os.path.join(TEST_FOLDER_PATH, "input_config_0_1_2.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_1_2_3.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_2_3_4.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_7_8_9.json")]
output = "merged_config.json"
reg_config = os.path.join(TEST_FOLDER_PATH, "example_registration.yaml")
with pytest.raises(ChannelOverlapError, match="channel graph is disconnected"):
config_merge(inputs=inputs, output=output, reg_config=reg_config)
# def test_non_overlapping_channel_error():
# """Add a non-overlapping config and show that it causes the right error"""
# inputs = [
# os.path.join(TEST_FOLDER_PATH, "input_config_0_1_2.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_1_2_3.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_2_3_4.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_7_8_9.json")]
# output = "merged_config.json"
# reg_config = os.path.join(TEST_FOLDER_PATH, "example_registration.yaml")
# with pytest.raises(ChannelOverlapError, match="channel graph is disconnected"):
# config_merge(inputs=inputs, output=output, reg_config=reg_config)

def test_unreadable_output_file():
"""Shows that a non-writable output configuration location causes the right error"""
inputs = [
os.path.join(TEST_FOLDER_PATH, "input_config_0_1_2.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_1_2_3.json"),
os.path.join(TEST_FOLDER_PATH, "input_config_2_3_4.json")]
output = "/root/merged_config.json"
reg_config = os.path.join(TEST_FOLDER_PATH, "example_registration.yaml")
with pytest.raises(OutputConfigWriteError, match="output file is not writable"):
config_merge(inputs=inputs, output=output, reg_config=reg_config)
# def test_unreadable_output_file():
# """Shows that a non-writable output configuration location causes the right error"""
# inputs = [
# os.path.join(TEST_FOLDER_PATH, "input_config_0_1_2.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_1_2_3.json"),
# os.path.join(TEST_FOLDER_PATH, "input_config_2_3_4.json")]
# output = "/root/merged_config.json"
# reg_config = os.path.join(TEST_FOLDER_PATH, "example_registration.yaml")
# with pytest.raises(OutputConfigWriteError, match="output file is not writable"):
# config_merge(inputs=inputs, output=output, reg_config=reg_config)

0 comments on commit 08e69ef

Please sign in to comment.