ceres-solver / ceres-solver / 646959ef118a1f10bf93741d97cf64265d42f8c6 / . / internal / ceres / corrector.h

// Ceres Solver - A fast non-linear least squares minimizer | |

// Copyright 2015 Google Inc. All rights reserved. | |

// http://ceres-solver.org/ | |

// | |

// Redistribution and use in source and binary forms, with or without | |

// modification, are permitted provided that the following conditions are met: | |

// | |

// * Redistributions of source code must retain the above copyright notice, | |

// this list of conditions and the following disclaimer. | |

// * Redistributions in binary form must reproduce the above copyright notice, | |

// this list of conditions and the following disclaimer in the documentation | |

// and/or other materials provided with the distribution. | |

// * Neither the name of Google Inc. nor the names of its contributors may be | |

// used to endorse or promote products derived from this software without | |

// specific prior written permission. | |

// | |

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" | |

// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE | |

// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE | |

// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE | |

// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR | |

// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF | |

// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS | |

// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN | |

// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) | |

// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | |

// POSSIBILITY OF SUCH DAMAGE. | |

// | |

// Author: sameeragarwal@google.com (Sameer Agarwal) | |

// | |

// Class definition for the object that is responsible for applying a | |

// second order correction to the Gauss-Newton based on the ideas in | |

// BANS by Triggs et al. | |

#ifndef CERES_INTERNAL_CORRECTOR_H_ | |

#define CERES_INTERNAL_CORRECTOR_H_ | |

namespace ceres { | |

namespace internal { | |

// Corrector is responsible for applying the second order correction | |

// to the residual and jacobian of a least squares problem based on a | |

// radial robust loss. | |

// | |

// The key idea here is to look at the expressions for the robustified | |

// gauss newton approximation and then take its square root to get the | |

// corresponding corrections to the residual and jacobian. For the | |

// full expressions see Eq. 10 and 11 in BANS by Triggs et al. | |

class Corrector { | |

public: | |

// The constructor takes the squared norm, the value, the first and | |

// second derivatives of the LossFunction. It precalculates some of | |

// the constants that are needed to apply the correction. The | |

// correction constant alpha is constrained to be smaller than 1, if | |

// it becomes larger than 1, then it will reverse the sign of the | |

// residual and the correction. If alpha is equal to 1 will result | |

// in a divide by zero error. Thus we constrain alpha to be upper | |

// bounded by 1 - epsilon_. | |

// | |

// rho[1] needs to be positive. The constructor will crash if this | |

// condition is not met. | |

// | |

// In practical use CorrectJacobian should always be called before | |

// CorrectResidual, because the jacobian correction depends on the | |

// value of the uncorrected residual values. | |

explicit Corrector(double sq_norm, const double rho[3]); | |

// residuals *= sqrt(rho[1]) / (1 - alpha) | |

void CorrectResiduals(int num_rows, double* residuals); | |

// jacobian = sqrt(rho[1]) * jacobian - | |

// sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. | |

// | |

// The method assumes that the jacobian has row-major storage. It is | |

// the caller's responsibility to ensure that the pointer to | |

// jacobian is not null. | |

void CorrectJacobian(int num_rows, | |

int num_cols, | |

double* residuals, | |

double* jacobian); | |

private: | |

double sqrt_rho1_; | |

double residual_scaling_; | |

double alpha_sq_norm_; | |

}; | |

} // namespace internal | |

} // namespace ceres | |

#endif // CERES_INTERNAL_CORRECTOR_H_ |