def setUp(self): """Set up a small Karcher mean optimization example.""" # Grabbed from KarcherMeanFactor unit tests. R = Rot3.Expmap(np.array([0.1, 0, 0])) rotations = {R, R.inverse()} # mean is the identity self.expected = Rot3() def check(actual): # Check that optimizing yields the identity self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) # Check that logging output prints out 3 lines (exact intermediate values differ by OS) self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3) # reset stdout catcher self.capturedOutput.truncate(0) self.check = check self.graph = gtsam.NonlinearFactorGraph() for R in rotations: self.graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) self.initial = gtsam.Values() self.initial.insert(KEY, R) # setup output capture self.capturedOutput = StringIO() sys.stdout = self.capturedOutput
def setUp(self): model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) # We consider a small graph: # symbolic FG # x2 0 1 # / | \ 1 2 # / | \ 2 3 # x3 | x1 2 0 # \ | / 0 3 # \ | / # x0 # p0 = Point3(0, 0, 0) self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) p1 = Point3(1, 2, 0) self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) p2 = Point3(0, 2, 0) self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) p3 = Point3(-1, 1, 0) self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) pose0 = Pose3(self.R0, p0) pose1 = Pose3(self.R1, p1) pose2 = Pose3(self.R2, p2) pose3 = Pose3(self.R3, p3) g = NonlinearFactorGraph() g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) g.add(gtsam.PriorFactorPose3(x0, pose0, model)) self.graph = g
def setUp(self): """Set up a small Karcher mean optimization example.""" # Grabbed from KarcherMeanFactor unit tests. R = Rot3.Expmap(np.array([0.1, 0, 0])) rotations = {R, R.inverse()} # mean is the identity self.expected = Rot3() graph = gtsam.NonlinearFactorGraph() for R in rotations: graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL)) initial = gtsam.Values() initial.insert(KEY, R) self.params = gtsam.GaussNewtonParams() self.optimizer = gtsam.GaussNewtonOptimizer(graph, initial, self.params) self.lmparams = gtsam.LevenbergMarquardtParams() self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer( graph, initial, self.lmparams) # setup output capture self.capturedOutput = StringIO() sys.stdout = self.capturedOutput
# pylint: disable=invalid-name, no-name-in-module, no-member import unittest import gtsam import numpy as np from gtsam import Rot3 from gtsam.utils.test_case import GtsamTestCase KEY = 0 MODEL = gtsam.noiseModel.Unit.Create(3) # Rot3 version R = Rot3.Expmap(np.array([0.1, 0, 0])) class TestKarcherMean(GtsamTestCase): def test_find(self): # Check that optimizing for Karcher mean (which minimizes Between distance) # gets correct result. rotations = gtsam.Rot3Vector([R, R.inverse()]) expected = Rot3() actual = gtsam.FindKarcherMean(rotations) self.gtsamAssertEquals(expected, actual) def test_find_karcher_mean_identity(self): """Averaging 3 identity rotations should yield the identity.""" a1Rb1 = Rot3()