diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index b3a334a92..64538f068 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -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);
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
index 087ae2b6d..08fe32002 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
@@ -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();