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()
