Go to the documentation of this file.
23 #include <gtsam/dllexport.h>
33 #include <Eigen/Sparse>
36 #include <type_traits>
41 class NonlinearFactorGraph;
42 class LevenbergMarquardtOptimizer;
49 using Anchor = std::pair<size_t, Rot>;
65 const std::string &method =
"JACOBI",
66 double optimalityThreshold = -1
e-4,
76 std::pair<size_t, Rot>
getAnchor()
const {
return anchor; }
94 void print(
const std::string &
s =
"")
const {
95 std::cout << (
s.empty() ?
s :
s +
" ");
96 std::cout <<
" ShonanAveragingParameters: " << std::endl;
97 std::cout <<
" alpha: " <<
alpha << std::endl;
98 std::cout <<
" beta: " <<
beta << std::endl;
99 std::cout <<
" gamma: " <<
gamma << std::endl;
100 std::cout <<
" useHuber: " << useHuber << std::endl;
172 return measurements_[k];
182 double k = 1.345)
const {
187 std::dynamic_pointer_cast<noiseModel::Robust>(
model);
192 robust_model =
model;
200 robustMeasurements.push_back(meas);
202 return robustMeasurements;
206 const Rot &
measured(
size_t k)
const {
return measurements_[k].measured(); }
215 Sparse D()
const {
return D_; }
217 Sparse Q()
const {
return Q_; }
219 Sparse L()
const {
return L_; }
232 return Matrix(computeLambda(
S));
254 Vector *minEigenVector =
nullptr)
const;
261 Vector *minEigenVector =
nullptr)
const;
277 const Vector &minEigenVector);
286 Values initializeWithDescent(
288 double minEigenValue,
double gradienTolerance = 1
e-2,
289 double preconditionedGradNormTolerance = 1
e-4)
const;
305 Values initializeRandomlyAt(
size_t p, std::mt19937 &
rng)
const;
308 Values initializeRandomlyAt(
size_t p)
const;
328 std::pair<double, Vector> computeMinEigenVector(
const Values &
values)
const;
342 std::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
369 for (
const auto& it :
values.extract<
T>()) {
392 Values initializeRandomly(std::mt19937 &
rng)
const;
395 Values initializeRandomly()
const;
404 std::pair<Values, double>
run(
const Values &initialEstimate,
size_t pMin =
d,
405 size_t pMax = 10)
const;
417 template <
typename T>
420 bool useRobustModel =
false)
const {
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
LevenbergMarquardtParams lm
LM parameters.
typedef and functions to augment Eigen's VectorXd
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
double getGaugesWeight() const
typename Parameters::Rot Rot
void setCertifyOptimality(bool value)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
typedef and functions to augment Eigen's MatrixXd
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
const GaussianFactorGraph factors
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
double alpha
weight of anchor-based prior (default 0)
void setKarcherWeight(double value)
bool getCertifyOptimality() const
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
Parameters for Levenberg-Marquardt trust-region scheme.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
double beta(double a, double b)
typename std::conditional< d==2, Rot2, Rot3 >::type Rot
double optimalityThreshold
threshold used in checkOptimality
3D rotation represented as a rotation matrix or quaternion
Measurements measurements_
Anchor anchor
pose to use as anchor if not Karcher
accelerated power method for fast eigenvalue and eigenvector computation
std::pair< size_t, Rot > getAnchor() const
double getKarcherWeight() const
Power method for fast eigenvalue and eigenvector computation.
utility functions for loading datasets
LevenbergMarquardtParams getLMParams() const
void setGaugesWeight(double value)
static ConjugateGradientParameters parameters
Parameters governing optimization etc.
double getAnchorWeight() const
void print(const std::string &s="") const
Print the parameters and flags used for rotation averaging.
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
noiseModel::Base::shared_ptr SharedNoiseModel
size_t numberMeasurements() const
Return number of measurements.
noiseModel::Diagonal::shared_ptr model
void setOptimalityThreshold(double value)
bool certifyOptimality
if enabled solution optimality is certified (default true)
size_t nrUnknowns() const
Return number of unknowns.
std::vector< BetweenFactor< Pose3 >::shared_ptr > BetweenFactorPose3s
static LevenbergMarquardtParams CeresDefaults()
std::vector< double > measurements
Eigen::SparseMatrix< double > Sparse
void setAnchorWeight(double value)
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p)
std::vector< BinaryMeasurement< Rot > > Measurements
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T >> &measurements, bool useRobustModel=false) const
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
Array< int, Dynamic, 1 > v
std::vector< BetweenFactor< Pose2 >::shared_ptr > BetweenFactorPose2s
std::pair< size_t, Rot > Anchor
void setUseHuber(bool value)
double beta
weight of Karcher-based prior (default 1)
static Point2 measurement(323.0, 240.0)
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
double getOptimalityThreshold() const
void setAnchor(size_t index, const Rot &value)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:15