12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update wrapper for LM with Ordering parameter

release/4.3a0
Varun Agrawal 2024-01-03 14:44:12 -05:00
parent 6f66d04f14
commit 0d058100e5
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -333,6 +333,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
const auto &factor = pair.second; const auto &factor = pair.second;
if (!factor) return 1.0; // TODO(dellaert): not loving this. if (!factor) return 1.0; // TODO(dellaert): not loving this.
// Logspace version of:
// exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); // exp(-factor->error(kEmpty)) / pair.first->normalizationConstant();
return -factor->error(kEmpty) - pair.first->logNormalizationConstant(); return -factor->error(kEmpty) - pair.first->logNormalizationConstant();
}; };
@ -345,6 +346,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors,
AlgebraicDecisionTree probabilities = DecisionTree<Key, double>( AlgebraicDecisionTree probabilities = DecisionTree<Key, double>(
logProbabilities, logProbabilities,
[&max_log](const double x) { return exp(x - max_log); }); [&max_log](const double x) { return exp(x - max_log); });
// probabilities.print("", DefaultKeyFormatter);
probabilities = probabilities.normalize(probabilities.sum()); probabilities = probabilities.normalize(probabilities.sum());
return { return {

11
gtsam/nonlinear/nonlinear.i
View File

@ -378,17 +378,22 @@ typedef gtsam::GncOptimizer<gtsam::GncParams<gtsam::LevenbergMarquardtParams>> G
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& initialValues); const gtsam::Values& initialValues,
const gtsam::LevenbergMarquardtParams& params =
gtsam::LevenbergMarquardtParams());
LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& initialValues, const gtsam::Values& initialValues,
const gtsam::LevenbergMarquardtParams& params); const gtsam::Ordering& ordering,
const gtsam::LevenbergMarquardtParams& params =
gtsam::LevenbergMarquardtParams());
double lambda() const; double lambda() const;
void print(string s = "") const; void print(string s = "") const;
}; };
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
class ISAM2GaussNewtonParams { class ISAM2GaussNewtonParams {
ISAM2GaussNewtonParams(); ISAM2GaussNewtonParams(double _wildfireThreshold = 0.001);
void print(string s = "") const; void print(string s = "") const;