def run(
self, num_images: int,
i2Ri1_dict: Dict[Tuple[int, int],
Optional[Rot3]]) -> List[Optional[Rot3]]:
"""Run the rotation averaging.
Args:
num_images: number of poses.
i2Ri1_dict: relative rotations as dictionary (i1, i2): i2Ri1.
Returns:
Global rotations for each camera pose, i.e. wRi, as a list. The number of entries in the list is
`num_images`. The list may contain `None` where the global rotation could not be computed (either
underconstrained system or ill-constrained system).
"""
lm_params = LevenbergMarquardtParams.CeresDefaults()
shonan_params = ShonanAveragingParameters3(lm_params)
shonan_params.setUseHuber(False)
shonan_params.setCertifyOptimality(True)
noise_model = gtsam.noiseModel.Unit.Create(6)
between_factors = gtsam.BetweenFactorPose3s()
for (i1, i2), i2Ri1 in i2Ri1_dict.items():
if i2Ri1 is not None:
between_factors.append(
BetweenFactorPose3(
i2,
i1,
Pose3(
i2Ri1,
np.zeros(3, ),
),
noise_model,
))
obj = ShonanAveraging3(between_factors, shonan_params)
initial = obj.initializeRandomly()
result_values, _ = obj.run(initial, self._p_min, self._p_max)
return [result_values.atRot3(i) for i in range(num_images)]
def test_PriorWeights(self):
lmParams = gtsam.LevenbergMarquardtParams.CeresDefaults()
params = ShonanAveragingParameters3(lmParams)
self.assertAlmostEqual(0, params.getAnchorWeight(), 1e-9)
self.assertAlmostEqual(1, params.getKarcherWeight(), 1e-9)
self.assertAlmostEqual(0, params.getGaugesWeight(), 1e-9)
alpha, beta, gamma = 100.0, 200.0, 300.0
params.setAnchorWeight(alpha)
params.setKarcherWeight(beta)
params.setGaugesWeight(gamma)
self.assertAlmostEqual(alpha, params.getAnchorWeight(), 1e-9)
self.assertAlmostEqual(beta, params.getKarcherWeight(), 1e-9)
self.assertAlmostEqual(gamma, params.getGaugesWeight(), 1e-9)
params.setKarcherWeight(0)
shonan = fromExampleName("Klaus3.g2o", params)
initial = gtsam.Values()
for i in range(3):
initial.insert(i, gtsam.Rot3())
self.assertAlmostEqual(3.0756, shonan.cost(initial), places=3)
result, _lambdaMin = shonan.run(initial, 3, 3)
self.assertAlmostEqual(0.0015, shonan.cost(result), places=3)
All Rights Reserved
See LICENSE for the license information
Unit tests for Shonan Rotation Averaging.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
import unittest
import gtsam
from gtsam import ShonanAveraging3, ShonanAveragingParameters3
from gtsam.utils.test_case import GtsamTestCase
DEFAULT_PARAMS = ShonanAveragingParameters3(
gtsam.LevenbergMarquardtParams.CeresDefaults())
class TestShonanAveraging(GtsamTestCase):
"""Tests for Shonan Rotation Averaging."""
def setUp(self):
"""Set up common variables."""
self.shonan = ShonanAveraging3("toyExample.g2o", DEFAULT_PARAMS)
def test_checkConstructor(self):
self.assertEqual(5, self.shonan.nrUnknowns())
D = self.shonan.denseD()
self.assertEqual((15, 15), D.shape)
Q = self.shonan.denseQ()
def __get_shonan_params(self) -> ShonanAveragingParameters3:
lm_params = LevenbergMarquardtParams.CeresDefaults()
shonan_params = ShonanAveragingParameters3(lm_params)
shonan_params.setUseHuber(False)
shonan_params.setCertifyOptimality(True)
return shonan_params