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

Add related code for reuse_diagonal, as Ceres did. This is for step strategry (efficiency comparison) in smartFactors_Ceres.

release/4.3a0
hchiu 2014-02-17 10:13:14 -05:00
parent c05eaa0d22
commit 13254bcc3a
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -116,9 +116,9 @@ void LevenbergMarquardtOptimizer::iterate() {
double sigma = 1.0 / std::sqrt(state_.lambda);
dampedSystem.reserve(dampedSystem.size() + state_.values.size());
// for each of the variables, add a prior
VectorValues hessianDiagonal;
if (params_.diagonalDamping) {
hessianDiagonal = linear->hessianDiagonal();
// Only retrieve diagonal vector when reuse_diagonal = false
if (params_.diagonalDamping && params_.reuse_diagonal_==false) {
state_.hessianDiagonal = linear->hessianDiagonal();
}
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
@ -126,10 +126,12 @@ void LevenbergMarquardtOptimizer::iterate() {
Matrix A = Matrix::Identity(dim, dim);
//Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping) {
A.diagonal() = hessianDiagonal.at(key_value.key);
A.diagonal() = state_.hessianDiagonal.at(key_value.key);
for (size_t aa=0; aa<dim; aa++)
{
A(aa,aa)= sqrt(std::min(std::max(A(aa,aa), min_diagonal_), max_diagonal_));
if (params_.reuse_diagonal_==false)
A(aa,aa)= std::min(std::max(A(aa,aa), min_diagonal_), max_diagonal_);
A(aa,aa)= sqrt(A(aa,aa));
}
}
Vector b = Vector::Zero(dim);

4
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -53,11 +53,12 @@ public:
double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
bool diagonalDamping; ///< if true, use diagonal of Hessian
bool reuse_diagonal_; //an additional option in Ceres for diagonalDamping (related to efficiency)
LevenbergMarquardtParams() :
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(
1e-3), diagonalDamping(false) {
1e-3), diagonalDamping(false), reuse_diagonal_(false) {
}
virtual ~LevenbergMarquardtParams() {
}
@ -118,6 +119,7 @@ public:
double lambda;
int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas)
boost::posix_time::ptime startTime;
VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false
LevenbergMarquardtState() {
initTime();