29 #ifdef GTSAM_USE_BOOST_FEATURES
44 typedef internal::LevenbergMarquardtState
State;
48 const Values& initialValues,
56 const Values& initialValues,
67 startTime_ = std::chrono::high_resolution_clock::now();
72 auto currentState =
static_cast<const State*
>(
state_.get());
73 return currentState->lambda;
78 auto currentState =
static_cast<const State*
>(
state_.get());
79 return currentState->totalNumberInnerIterations;
91 auto currentState =
static_cast<const State*
>(
state_.get());
94 std::cout <<
"building damped system with lambda " << currentState->lambda << std::endl;
97 return currentState->buildDampedSystem(linear, sqrtHessianDiagonal);
99 return currentState->buildDampedSystem(linear);
105 auto currentState =
static_cast<const State*
>(
state_.get());
110 auto currentTime = std::chrono::high_resolution_clock::now();
112 double timeSpent = std::chrono::duration_cast<std::chrono::microseconds>(currentTime -
startTime_).count() / 1e6;
113 os << currentState->totalNumberInnerIterations <<
","
115 << currentError <<
"," << currentState->lambda <<
","
116 << currentState->iterations << endl;
123 auto currentState =
static_cast<const State*
>(
state_.get());
126 #ifdef GTSAM_USE_BOOST_FEATURES
127 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
128 boost::timer::cpu_timer lamda_iteration_timer;
129 lamda_iteration_timer.start();
132 lamda_iteration_timer.restart();
135 auto start = std::chrono::high_resolution_clock::now();
139 cout <<
"trying lambda = " << currentState->lambda << endl;
145 double modelFidelity = 0.0;
146 bool step_is_successful =
false;
147 bool stopSearchingLambda =
false;
148 double newError = numeric_limits<double>::infinity();
149 double costChange = 0.0;
153 bool systemSolvedSuccessfully;
157 systemSolvedSuccessfully =
true;
159 systemSolvedSuccessfully =
false;
162 if (systemSolvedSuccessfully) {
164 cout <<
"linear delta norm = " <<
delta.norm() << endl;
166 delta.print(
"delta");
171 double newlinearizedError = linear.
error(
delta);
174 double linearizedCostChange = oldLinearizedError - newlinearizedError;
176 cout <<
"newlinearizedError = " << newlinearizedError
177 <<
" linearizedCostChange = " << linearizedCostChange << endl;
179 if (linearizedCostChange >= 0) {
188 gttic(compute_error);
190 cout <<
"calculating error:" << endl;
192 gttoc(compute_error);
195 cout <<
"old error (" << currentState->error <<
") new (tentative) error (" << newError
199 costChange = currentState->error - newError;
204 modelFidelity = costChange / linearizedCostChange;
208 cout <<
"modelFidelity: " << modelFidelity << endl;
214 if (
std::abs(costChange) < minAbsoluteTolerance) {
216 cout <<
"abs(costChange)=" <<
std::abs(costChange)
217 <<
" minAbsoluteTolerance=" << minAbsoluteTolerance
219 stopSearchingLambda =
true;
225 #ifdef GTSAM_USE_BOOST_FEATURES
227 #ifdef GTSAM_USING_NEW_BOOST_TIMERS
228 double iterationTime = 1
e-9 * lamda_iteration_timer.elapsed().wall;
230 double iterationTime = lamda_iteration_timer.elapsed();
233 auto end = std::chrono::high_resolution_clock::now();
234 double iterationTime = std::chrono::duration_cast<std::chrono::microseconds>(
end - start).count() / 1e6;
236 if (currentState->iterations == 0) {
237 cout <<
"iter cost cost_change lambda success iter_time" << endl;
239 cout << setw(4) << currentState->iterations <<
" " << setw(12) << newError <<
" " << setw(12) << setprecision(2)
240 << costChange <<
" " << setw(10) << setprecision(2) << currentState->lambda <<
" " << setw(6)
241 << systemSolvedSuccessfully <<
" " << setw(10) << setprecision(2) << iterationTime << endl;
243 if (step_is_successful) {
247 state_ = currentState->decreaseLambda(
params_, modelFidelity, std::move(newValues), newError);
249 }
else if (!stopSearchingLambda) {
251 cout <<
"increasing lambda" << endl;
253 modifiedState->increaseLambda(
params_);
259 cout <<
"Warning: Levenberg-Marquardt giving up because "
260 "cannot decrease error with maximum lambda" << endl;
267 cout <<
"Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
274 auto currentState =
static_cast<const State*
>(
state_.get());
280 cout <<
"linearizing = " << endl;
283 if(currentState->totalNumberInnerIterations==0) {
287 cout <<
"Initial error: " << currentState->error
288 <<
", values: " << currentState->values.size() << std::endl;
295 sqrtHessianDiagonal = linear->hessianDiagonal();
296 for (
auto& [
key,
value] : sqrtHessianDiagonal) {
302 while (!
tryLambda(*linear, sqrtHessianDiagonal)) {
303 auto newState =
static_cast<const State*
>(
state_.get());