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); double sigma = 1.0 / std::sqrt(state_.lambda);
dampedSystem.reserve(dampedSystem.size() + state_.values.size()); dampedSystem.reserve(dampedSystem.size() + state_.values.size());
// for each of the variables, add a prior // for each of the variables, add a prior
VectorValues hessianDiagonal; // Only retrieve diagonal vector when reuse_diagonal = false
if (params_.diagonalDamping) { if (params_.diagonalDamping && params_.reuse_diagonal_==false) {
hessianDiagonal = linear->hessianDiagonal(); state_.hessianDiagonal = linear->hessianDiagonal();
} }
BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) { BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
@ -126,10 +126,12 @@ void LevenbergMarquardtOptimizer::iterate() {
Matrix A = Matrix::Identity(dim, dim); Matrix A = Matrix::Identity(dim, dim);
//Replace the identity matrix with diagonal of Hessian //Replace the identity matrix with diagonal of Hessian
if (params_.diagonalDamping) { 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++) 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); 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 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] std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
bool diagonalDamping; ///< if true, use diagonal of Hessian bool diagonalDamping; ///< if true, use diagonal of Hessian
bool reuse_diagonal_; //an additional option in Ceres for diagonalDamping (related to efficiency)
LevenbergMarquardtParams() : LevenbergMarquardtParams() :
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity( 0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(
1e-3), diagonalDamping(false) { 1e-3), diagonalDamping(false), reuse_diagonal_(false) {
} }
virtual ~LevenbergMarquardtParams() { virtual ~LevenbergMarquardtParams() {
} }
@ -118,6 +119,7 @@ public:
double lambda; double lambda;
int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) 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; boost::posix_time::ptime startTime;
VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false
LevenbergMarquardtState() { LevenbergMarquardtState() {
initTime(); initTime();