Covariance definition in test_Pose3SLAMExample.py

[gogojjh]

Description

gtsam/python/gtsam/tests/test_Pose3SLAMExample.py: Is it something wrong wit the the definition of covariance (line 33):

covariance = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)]))

According to the definition of noisemodel for Pose3, the correct definition should be

covariance = gtsam.noiseModel.Diagonal.Sigmas(np.array([roll_std, pitch_std, yaw_std, tx_std, ty_std, tz_std]))

i.e.,

covariance = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.)]))

[varunagrawal]

Hi @gogojjh. Did you mean the noise model should instead be the below?

covariance = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(5.), np.deg2rad(5.), np.deg2rad(5.), 0.05, 0.05, 0.05]))

I believe you are correct since the Logmap(Pose3) returns [rot, trans]. Would you like to create a PR for this and I can review and merge it?

[gogojjh]

Sure