Go to the documentation of this file.
43 template<
class GncParameters>
59 const GncParameters&
params = GncParameters())
65 for (
size_t i = 0;
i <
graph.size();
i++) {
69 std::dynamic_pointer_cast<noiseModel::Robust>(factor->noiseModel());
71 nfg_[
i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
76 std::vector<size_t> inconsistentlySpecifiedWeights;
78 std::set_intersection(
params.knownInliers.begin(),
params.knownInliers.end(),
80 std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin()));
81 if(inconsistentlySpecifiedWeights.size() > 0){
83 throw std::runtime_error(
"GncOptimizer::constructor: the user has selected one or more measurements"
84 " to be BOTH a known inlier and a known outlier.");
87 for (
size_t i = 0;
i <
params.knownInliers.size();
i++){
89 throw std::runtime_error(
"GncOptimizer::constructor: the user has selected one or more measurements"
90 "that are not in the factor graph to be known inliers.");
94 for (
size_t i = 0;
i <
params.knownOutliers.size();
i++){
96 throw std::runtime_error(
"GncOptimizer::constructor: the user has selected one or more measurements"
97 "that are not in the factor graph to be known outliers.");
132 for (
size_t k = 0; k <
nfg_.
size(); k++) {
144 throw std::runtime_error(
145 "GncOptimizer::setWeights: the number of specified weights"
146 " does not match the size of the factor graph.");
176 for (
size_t i = 0;
i <
params_.knownOutliers.size();
i++){
177 weights[
params_.knownOutliers[
i] ] = 0.0;
198 if (
mu <= 0 || nrUnknownInOrOut == 0) {
199 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
200 std::cout <<
"GNC Optimizer stopped because maximum residual at "
201 "initialization is small."
204 if (nrUnknownInOrOut==0 &&
params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
205 std::cout <<
"GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
208 if (
params_.verbosity >= GncParameters::Verbosity::MU) {
209 std::cout <<
"mu: " <<
mu << std::endl;
211 if (
params_.verbosity >= GncParameters::Verbosity::VALUES) {
221 if (
params_.verbosity >= GncParameters::Verbosity::MU) {
222 std::cout <<
"iter: " <<
iter << std::endl;
223 std::cout <<
"mu: " <<
mu << std::endl;
225 if (
params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
226 std::cout <<
"weights: " <<
weights_ << std::endl;
228 if (
params_.verbosity >= GncParameters::Verbosity::VALUES) {
238 result = baseOptimizer_iter.optimize();
253 if (
params_.verbosity >= GncParameters::Verbosity::VALUES) {
254 std::cout <<
"previous cost: " << prev_cost << std::endl;
255 std::cout <<
"current cost: " << cost << std::endl;
259 if (
params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
260 std::cout <<
"final iterations: " <<
iter << std::endl;
261 std::cout <<
"final mu: " <<
mu << std::endl;
262 std::cout <<
"previous cost: " << prev_cost << std::endl;
263 std::cout <<
"current cost: " << cost << std::endl;
265 if (
params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
266 std::cout <<
"final weights: " <<
weights_ << std::endl;
274 double mu_init = 0.0;
281 for (
size_t k = 0; k <
nfg_.
size(); k++) {
294 mu_init = std::numeric_limits<double>::infinity();
295 for (
size_t k = 0; k <
nfg_.
size(); k++) {
298 mu_init = (2 * rk -
barcSq_[k]) > 0 ?
302 if (mu_init >= 0 && mu_init < 1
e-6){
307 return mu_init > 0 && !
std::isinf(mu_init) ? mu_init : -1;
311 throw std::runtime_error(
312 "GncOptimizer::initializeMu: called with unknown loss type.");
326 throw std::runtime_error(
327 "GncOptimizer::updateMu: called with unknown loss type.");
333 bool muConverged =
false;
342 throw std::runtime_error(
343 "GncOptimizer::checkMuConvergence: called with unknown loss type.");
345 if (muConverged &&
params_.verbosity >= GncParameters::Verbosity::SUMMARY)
346 std::cout <<
"muConverged = true " << std::endl;
354 if (costConverged &&
params_.verbosity >= GncParameters::Verbosity::SUMMARY){
355 std::cout <<
"checkCostConvergence = true (prev. cost = " << prev_cost
356 <<
", curr. cost = " << cost <<
")" << std::endl;
358 return costConverged;
363 bool weightsConverged =
false;
366 weightsConverged =
false;
369 weightsConverged =
true;
370 for (
int i = 0;
i < weights.size();
i++) {
373 weightsConverged =
false;
379 throw std::runtime_error(
380 "GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
383 &&
params_.verbosity >= GncParameters::Verbosity::SUMMARY)
384 std::cout <<
"weightsConverged = true " << std::endl;
385 return weightsConverged;
390 const double cost,
const double prev_cost)
const {
403 auto noiseModel = std::dynamic_pointer_cast<noiseModel::Gaussian>(
404 factor->noiseModel());
406 Matrix newInfo = weights[
i] * noiseModel->information();
408 newGraph[
i] = factor->cloneWithNewNoiseModel(newNoiseModel);
410 throw std::runtime_error(
411 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
423 std::vector<size_t> allWeights;
424 for (
size_t k = 0; k <
nfg_.
size(); k++) {
425 allWeights.push_back(k);
427 std::vector<size_t> knownWeights;
428 std::set_union(
params_.knownInliers.begin(),
params_.knownInliers.end(),
430 std::inserter(knownWeights, knownWeights.begin()));
432 std::vector<size_t> unknownWeights;
433 std::set_difference(allWeights.begin(), allWeights.end(),
434 knownWeights.begin(), knownWeights.end(),
435 std::inserter(unknownWeights, unknownWeights.begin()));
440 for (
size_t k : unknownWeights) {
442 double u2_k =
nfg_[k]->
error(currentEstimate);
450 for (
size_t k : unknownWeights) {
452 double u2_k =
nfg_[k]->
error(currentEstimate);
456 if (u2_k >= upperbound || weights[k] < 0) {
458 }
else if (u2_k <= lowerbound || weights[k] > 1) {
466 throw std::runtime_error(
467 "GncOptimizer::calculateWeights: called with unknown loss type.");
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const Vector & getWeights() const
Access a copy of the GNC weights.
GncParameters params_
GNC parameters.
std::shared_ptr< This > shared_ptr
double updateMu(const double mu) const
Update the gnc parameter mu to gradually increase nonconvexity.
const Vector & getInlierCostThresholds() const
Get the inlier threshold.
static const SmartProjectionParams params
static double Chi2inv(const double alpha, const size_t dofs)
Vector weights_
Weights associated to each factor in GNC (this could be a local variable in optimize,...
bool equals(const GncOptimizer &other, double tol=1e-9) const
Equals.
const sharedFactor at(size_t i) const
double error(const Values &values) const
virtual void resize(size_t size)
bool checkCostConvergence(const double cost, const double prev_cost) const
Check convergence of relative cost differences.
Vector calculateWeights(const Values ¤tEstimate, const double mu)
Calculate gnc weights.
const GncParameters & getParams() const
Access a copy of the parameters.
static shared_ptr Information(const Matrix &M, bool smart=true)
GncParameters::OptimizerType BaseOptimizer
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
void setInlierCostThresholds(const double inth)
double chi_squared_quantile(const double dofs, const double alpha)
Compute the quantile function of the Chi-Squared distribution.
NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const
Create a graph where each factor is weighted by the gnc weights.
NonlinearFactorGraph nfg_
Original factor graph to be solved by GNC.
Jet< T, N > pow(const Jet< T, N > &f, double g)
Implementation of the Chi Squared inverse function.
bool checkWeightsConvergence(const Vector &weights) const
Check convergence of weights to binary values.
bool checkMuConvergence(const double mu) const
Check if we have reached the value of mu for which the surrogate loss matches the original loss.
Vector initializeWeightsFromKnownInliersAndOutliers() const
Vector barcSq_
Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq_[i] (where i is the po...
Factor Graph consisting of non-linear factors.
const NonlinearFactorGraph & getFactors() const
Access a copy of the internal factor graph.
iterator iter(handle obj)
double initializeMu() const
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
Values optimize()
Compute optimal solution using graduated non-convexity.
void setWeights(const Vector w)
Values state_
Initial values to be used at each iteration by GNC.
NonlinearFactorGraph graph
GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters ¶ms=GncParameters())
Constructor.
bool equal(const T &obj1, const T &obj2, double tol)
bool checkConvergence(const double mu, const Vector &weights, const double cost, const double prev_cost) const
Check for convergence between consecutive GNC iterations.
Jet< T, N > sqrt(const Jet< T, N > &f)
const Values & getState() const
Access a copy of the internal values.
void setInlierCostThresholdsAtProbability(const double alpha)
void setInlierCostThresholds(const Vector &inthVec)
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:23