Go to the documentation of this file.
31 using namespace gtsam;
40 const std::string &
name,
41 ShonanAveraging3::Parameters
parameters = ShonanAveraging3::Parameters()) {
82 Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations);
119 Values initial = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
134 Values random = ShonanAveraging3::LiftTo<Rot3>(4, randomRotations);
148 v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
149 Vector expected0(10), expected1(10), expected2(10);
150 expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0;
151 expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0;
152 expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0;
170 static std::mt19937
rng(0);
171 Vector descentDirection = Vector::Random(15);
173 Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
182 const Matrix S = ShonanAveraging3::StiefelElementMatrix(Qstar3);
184 bool computeEigenvectors =
false;
187 double minEigenValue = lambdas(0);
188 for (
int i = 1;
i < lambdas.size();
i++)
189 minEigenValue =
min(lambdas(
i), minEigenValue);
205 ShonanAveraging3::LiftwithDescent(4, Qstar3, descentDirection);
221 Values random = ShonanAveraging3::LiftTo<Rot3>(3, randomRotations);
242 const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624,
243 -0.015064701622500615);
244 const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356,
245 -0.004386230477751116);
246 const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454,
247 -0.04088089479075661);
251 using namespace klaus;
266 static const Rot3 identity;
278 -0.01796327847857683, 0.010210006313668573),
309 using namespace klaus;
334 auto lmParams = LevenbergMarquardtParams::CeresDefaults();
354 auto lmParams = LevenbergMarquardtParams::CeresDefaults();
361 string parameters_print =
362 " ShonanAveragingParameters: \n alpha: 0\n beta: 1\n gamma: 0\n "
374 for (
size_t i=0;
i<=4;
i++) {
375 const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
392 auto lmParams = LevenbergMarquardtParams::CeresDefaults();
404 params.setKarcherWeight(0);
406 for (
auto i : {0,1,2}) {
409 std::static_pointer_cast<noiseModel::Isotropic>(
m.noiseModel());
410 CHECK(isotropic !=
nullptr);
425 auto unit3 = noiseModel::Unit::Create(3);
static int runAllTests(TestResult &result)
Class between(const Class &g) const
double computeMinEigenValueAP(const Values &values, Vector *minEigenVector=nullptr) const
Annotation for function names.
Shonan Averaging algorithm.
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Sparse computeLambda(const Matrix &S) const
Version that takes pxdN Stiefel manifold elements.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT_LONGS_EQUAL(expected, actual)
#define EXPECT(condition)
bool checkOptimality(const Values &values) const
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
LevenbergMarquardtParams lmParams
Values roundSolution(const Values &values) const
GenericValue< T > genericValue(const T &v)
static const SmartProjectionParams params
double beta(double a, double b)
static const ShonanAveraging3 kShonan
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
Provides additional testing facilities for common data structures.
Values initializeRandomly(std::mt19937 &rng) const
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
const sharedFactor at(size_t i) const
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
const ValueType at(Key j) const
Sparse computeA(const Values &values) const
Compute A matrix whose Eigenvalues we will examine.
Values initializeWithDescent(size_t p, const Values &values, const Vector &minEigenVector, double minEigenValue, double gradienTolerance=1e-2, double preconditionedGradNormTolerance=1e-4) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
const Rot3 wR2(0.9925378735259738, -0.07993768981394891, 0.0825062894866454, -0.04088089479075661)
const SharedNoiseModel & noiseModel() const
access to the noise model
static ConjugateGradientParameters parameters
ShonanAveraging3 fromExampleName(const std::string &name, ShonanAveraging3::Parameters parameters=ShonanAveraging3::Parameters())
double computeMinEigenValue(const Values &values, Vector *minEigenVector=nullptr) const
TEST(ShonanAveraging3, checkConstructor)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Rot3 inverse() const
inverse of a rotation
size_t nrUnknowns() const
Return number of unknowns.
double costAt(size_t p, const Values &values) const
The quaternion class used to represent 3D orientations and rotations.
const Rot3 wR1(0.998239108728862, -0.049543805396343954, -0.03232420352077356, -0.004386230477751116)
static LevenbergMarquardtParams CeresDefaults()
ShonanAveragingParameters< 3 > ShonanAveragingParameters3
std::vector< double > measurements
std::pair< Values, double > run(const Values &initialEstimate, size_t pMin=d, size_t pMax=10) const
static std::mt19937 kRandomNumberGenerator(42)
Computes eigenvalues and eigenvectors of general matrices.
Array< int, Dynamic, 1 > v
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
double cost(const Values &values) const
Values tryOptimizingAt(size_t p, const Values &initial) const
NonlinearFactorGraph buildGraphAt(size_t p) const
NonlinearFactorGraph graph
const Rot3 wR0(0.9992281076190063, -0.02676080288219576, -0.024497002638379624, -0.015064701622500615)
Various factors that minimize some Frobenius norm.
static Unit3 unit3(1.0, 2.1, 3.4)
const EigenvalueType & eigenvalues() const
Returns the eigenvalues of given matrix.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:41:35