2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 KarcherMeanFactor unit tests.
20 from gtsam
import Rot3
26 R = Rot3.Expmap(np.array([0.1, 0, 0]))
33 Check that optimizing for Karcher mean (which minimizes Between distance)
36 rotations = [R, R.inverse()]
42 """Averaging 3 identity rotations should yield the identity."""
47 aRb_list = [a1Rb1, a2Rb2, a3Rb3]
54 """Check that the InnerConstraint factor leaves the mean unchanged."""
61 R12 = R.compose(R.compose(R))
62 graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
64 graph.add(gtsam.KarcherMeanFactorRot3(keys))
67 initial.insert(1, R.inverse())
77 if __name__ ==
"__main__":