<jupyter_start><jupyter_text>Create data<jupyter_code>import copy
from liegroups import SE2, SO2

params_true = {'T_1_0': SE2.identity(),
               'T_2_0': SE2(SO2.identity(), -np.array([0.5, 0])),
               'T_3_0': SE2(SO2.identity(), -np.array([1, 0])),
               'T_4_0': SE2(SO2.from_angle(np.pi / 2),
                             -(SO2.from_angle(np.pi / 2).dot(np.array([1, 0.5])))),
               'T_5_0': SE2(SO2.from_angle(np.pi),
                             -(SO2.from_angle(np.pi).dot(np.array([0.5, 0.5])))),
               'T_6_0': SE2(SO2.from_angle(-np.pi / 2),
                             -(SO2.from_angle(-np.pi / 2).dot(np.array([0.5, 0]))))}

# observation: relative pose between poses
obs = {'T_1_0': params_true['T_1_0'],
       'T_2_1': params_true['T_2_0'].dot(params_true['T_1_0'].inv()),
       'T_3_2': params_true['T_3_0'].dot(params_true['T_2_0'].inv()),
       'T_4_3': params_true['T_4_0'].dot(params_true['T_3_0'].inv()),
       'T_5_4': params_true['T_5_0'].dot(params_true['T_4_0'].inv()),
       'T_6_5': params_true['T_6_0'].dot(params_true['T_5_0'].inv()),
       'T_6_2': params_true['T_6_0'].dot(params_true['T_2_0'].inv())}

# parans_init直接设为 SE3.exp(np.random.rand(3))是否可以?
params_init = copy.deepcopy(params_true)
for key in params_init.keys():
    params_init[key] = SE2.exp(5 * np.random.rand(3)).dot(params_init[key])<jupyter_output><empty_output><jupyter_text>Create residual functions<jupyter_code>from pyslam.residuals import PoseResidual, PoseToPoseResidual
from pyslam.utils import invsqrt

# Q: stiffness的值是如何设定的?
prior_stiffness = invsqrt(1e-12 * np.identity(3))
odom_stiffness = invsqrt(1e-3 * np.identity(3))
loop_stiffness = invsqrt(1e-3 * np.identity(3))

residual0 = PoseResidual(obs['T_1_0'], prior_stiffness)
residual0_params = ['T_1_0']

residual1 = PoseToPoseResidual(obs['T_2_1'], odom_stiffness)
residual1_params = ['T_1_0', 'T_2_0']

residual2 = PoseToPoseResidual(obs['T_3_2'], odom_stiffness)
residual2_params = ['T_2_0', 'T_3_0']

residual3 = PoseToPoseResidual(obs['T_4_3'], odom_stiffness)
residual3_params = ['T_3_0', 'T_4_0']

residual4 = PoseToPoseResidual(obs['T_5_4'], odom_stiffness)
residual4_params = ['T_4_0', 'T_5_0']

residual5 = PoseToPoseResidual(obs['T_6_5'], odom_stiffness)
residual5_params = ['T_5_0', 'T_6_0']

# loop closure
residual6 = PoseToPoseResidual(obs['T_6_2'], loop_stiffness)
residual6_params = ['T_2_0', 'T_6_0']
prior_stiffness, odom_stiffness, loop_stiffness<jupyter_output><empty_output><jupyter_text>Set up and solve the problem<jupyter_code>from pyslam.problem import Problem, Options

options = Options()
options.allow_nondecreasing_steps = True
options.max_nondecreasing_steps = 3

problem = Problem(options)

problem.add_residual_block(residual0, residual0_params)
problem.add_residual_block(residual1, residual1_params)
problem.add_residual_block(residual2, residual2_params)
problem.add_residual_block(residual3, residual3_params)
problem.add_residual_block(residual4, residual4_params)
problem.add_residual_block(residual5, residual5_params)
# problem.add_residual_block(residual6, residual6_params)

problem.initialize_params(params_init)

params_final = problem.solve()
print(problem.summary(format='full'))<jupyter_output>Iter | Initial cost -->   Final cost | Rel change
---------------------------------------------------
    0 | 6.606496e+12 --> 9.532377e+04 |  -1.000000
    1 | 9.532377e+04 --> 2.505429e-23 |  -1.000000<jupyter_text>Check results<jupyter_code>print("Initial Error:")
for key in params_true.keys():
    print('{}: {}'.format(key, SE2.log(params_init[key].inv().dot(params_true[key]))))

print()

print("Final Error:")
for key in params_true.keys():
    print('{}: {}'.format(key, SE2.log(params_final[key].inv().dot(params_true[key]))))<jupyter_output>Initial Error:
T_1_0: [-2.74750539 -0.11280802 -2.37728413]
T_2_0: [ 0.79095551 -0.18835619  2.01045808]
T_3_0: [1.04526652 0.40307472 2.34758801]
T_4_0: [-4.55587979  5.44361476 -1.74972239]
T_5_0: [-0.76427681 -2.37838095  2.78393503]
T_6_0: [ 0.70271386 -3.26183532 -1.08983471]

Final Error:
T_1_0: [-8.27180810e-25  4.93038066e-31  0.00000000e+00]
T_2_0: [-7.77156117e-16  6.66133815e-16  1.11022302e-16]
T_3_0: [-1.88737914e-15  4.44089210e-15 -5.92670972e-16]
T_4_0: [ 1.59872116e-14 -1.14908083e-14  1.39580076e-16]
T_5_0: [-3.78030940e-14  1.03916875e-13  3.36086006e-15]
T_6_0: [-1.11022302e-13  2.69531534e-13  3.36086006e-15]<jupyter_text>Optional: Compute the covariance of the final parameter estimates<jupyter_code>problem.compute_covariance()
print('covariance of T_5_0:\n{}'.format( problem.get_covariance_block('T_5_0','T_5_0') ))<jupyter_output>covariance of T_5_0:
[[0.0045  0.00025 0.001  ]
 [0.00025 0.0045  0.001  ]
 [0.001   0.001   0.004  ]]
{'fraction_non_alphanumeric': 0.17246437552388935, 'fraction_numerical': 0.13809723386420789, 'mean_word_length': 4.668646080760095, 'pattern_counts': {'":': 0, '<': 18, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 1, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '5', 'dir': 'jupyter-structured-clean-dedup', 'id': '24119', 'license': 'MIT', 'n_tokens_mistral': 2395, 'n_tokens_neox': 2070, 'n_words': 276, 'path': 'examples/Pose graph relaxation in SE(2).ipynb', 'repo_name': 'angshine/pyslam', 'size': '4772'}