diff options
author | Angus Kong <shkong@google.com> | 2013-02-13 14:56:04 -0800 |
---|---|---|
committer | Angus Kong <shkong@google.com> | 2013-02-14 13:33:05 -0800 |
commit | 0ae28bd5885b5daa526898fcf7c323dc2c3e1963 (patch) | |
tree | 487c4897ae7509fb4e05751b0a673a8090e26d41 /internal/ceres/corrector.cc | |
parent | e9e9be9383b479ac1573370ab221ffbac4de8c90 (diff) | |
download | ceres-solver-0ae28bd5885b5daa526898fcf7c323dc2c3e1963.tar.gz |
Initial import of ceres-solver 1.4.0android-4.3_r3.1android-4.3_r3android-4.3_r2.3android-4.3_r2.2android-4.3_r2.1android-4.3_r2android-4.3_r1.1android-4.3_r1android-4.3_r0.9.1android-4.3_r0.9android-4.3.1_r1tools_r22.2jb-mr2.0.0-releasejb-mr2.0-releasejb-mr2-releasejb-mr2-dev
Added a NOTICE and a MODULE_LICENSE_BSD file.
Added Android.mk for master build and unbundled build.
Added CleanSpec.mk to optimize Android build.
Change-Id: I6cd82bcabc1a94b10239f9fca017de0afd20e769
Diffstat (limited to 'internal/ceres/corrector.cc')
-rw-r--r-- | internal/ceres/corrector.cc | 125 |
1 files changed, 125 insertions, 0 deletions
diff --git a/internal/ceres/corrector.cc b/internal/ceres/corrector.cc new file mode 100644 index 0000000..eff4dff --- /dev/null +++ b/internal/ceres/corrector.cc @@ -0,0 +1,125 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// 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) + +#include "ceres/corrector.h" + +#include <cstddef> +#include <cmath> +#include "ceres/internal/eigen.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +Corrector::Corrector(double sq_norm, const double rho[3]) { + CHECK_GE(sq_norm, 0.0); + CHECK_GT(rho[1], 0.0); + sqrt_rho1_ = sqrt(rho[1]); + + // If sq_norm = 0.0, the correction becomes trivial, the residual + // and the jacobian are scaled by the squareroot of the derivative + // of rho. Handling this case explicitly avoids the divide by zero + // error that would occur below. + // + // The case where rho'' < 0 also gets special handling. Technically + // it shouldn't, and the computation of the scaling should proceed + // as below, however we found in experiments that applying the + // curvature correction when rho'' < 0, which is the case when we + // are in the outlier region slows down the convergence of the + // algorithm significantly. + // + // Thus, we have divided the action of the robustifier into two + // parts. In the inliner region, we do the full second order + // correction which re-wights the gradient of the function by the + // square root of the derivative of rho, and the Gauss-Newton + // Hessian gets both the scaling and the rank-1 curvature + // correction. Normaly, alpha is upper bounded by one, but with this + // change, alpha is bounded above by zero. + // + // Empirically we have observed that the full Triggs correction and + // the clamped correction both start out as very good approximations + // to the loss function when we are in the convex part of the + // function, but as the function starts transitioning from convex to + // concave, the Triggs approximation diverges more and more and + // ultimately becomes linear. The clamped Triggs model however + // remains quadratic. + // + // The reason why the Triggs approximation becomes so poor is + // because the curvature correction that it applies to the gauss + // newton hessian goes from being a full rank correction to a rank + // deficient correction making the inversion of the Hessian fraught + // with all sorts of misery and suffering. + // + // The clamped correction retains its quadratic nature and inverting it + // is always well formed. + if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { + residual_scaling_ = sqrt_rho1_; + alpha_sq_norm_ = 0.0; + return; + } + + // Calculate the smaller of the two solutions to the equation + // + // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. + // + // Start by calculating the discriminant D. + const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + + // Since both rho[1] and rho[2] are guaranteed to be positive at + // this point, we know that D > 1.0. + + const double alpha = 1.0 - sqrt(D); + + // Calculate the constants needed by the correction routines. + residual_scaling_ = sqrt_rho1_ / (1 - alpha); + alpha_sq_norm_ = alpha / sq_norm; +} + +void Corrector::CorrectResiduals(int nrow, double* residuals) { + DCHECK(residuals != NULL); + VectorRef r_ref(residuals, nrow); + // Equation 11 in BANS. + r_ref *= residual_scaling_; +} + +void Corrector::CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian) { + DCHECK(residuals != NULL); + DCHECK(jacobian != NULL); + ConstVectorRef r_ref(residuals, nrow); + MatrixRef j_ref(jacobian, nrow, ncol); + + // Equation 11 in BANS. + j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * + r_ref * (r_ref.transpose() * j_ref)); +} + +} // namespace internal +} // namespace ceres |