search

borglab / gtsam

GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices.
http://gtsam.org
Other
2.61k stars 765 forks source link

Covariance definition in test_Pose3SLAMExample.py #1793

Open gogojjh opened 2 months ago

gogojjh commented 2 months ago

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 commented 2 months ago

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 commented 2 months ago

Sure