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(
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 {
421 return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
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
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
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()
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 Sat Sep 28 2024 03:02:26