Various cleanups Change-Id: Ie453e199a128a102bf3d5524a466f549aac0c7ce
diff --git a/examples/slam/common/read_g2o.h b/examples/slam/common/read_g2o.h index 2999339..b270e28 100644 --- a/examples/slam/common/read_g2o.h +++ b/examples/slam/common/read_g2o.h
@@ -34,6 +34,7 @@ #define EXAMPLES_CERES_READ_G2O_H_ #include <fstream> +#include <map> #include <string> #include "absl/log/check.h"
diff --git a/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h b/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h index 3d34f8d..7c1be18 100644 --- a/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h +++ b/examples/slam/pose_graph_2d/pose_graph_2d_error_term.h
@@ -35,6 +35,7 @@ #include "Eigen/Core" #include "ceres/autodiff_cost_function.h" +#include "normalize_angle.h" namespace ceres::examples {
diff --git a/include/ceres/internal/line_parameterization.h b/include/ceres/internal/line_parameterization.h deleted file mode 100644 index f50603d..0000000 --- a/include/ceres/internal/line_parameterization.h +++ /dev/null
@@ -1,183 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2023 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: jodebo_beck@gmx.de (Johannes Beck) -// - -#ifndef CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ -#define CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ - -#include "householder_vector.h" - -namespace ceres { - -template <int AmbientSpaceDimension> -bool LineParameterization<AmbientSpaceDimension>::Plus( - const double* x_ptr, - const double* delta_ptr, - double* x_plus_delta_ptr) const { - // We seek a box plus operator of the form - // - // [o*, d*] = Plus([o, d], [delta_o, delta_d]) - // - // where o is the origin point, d is the direction vector, delta_o is - // the delta of the origin point and delta_d the delta of the direction and - // o* and d* is the updated origin point and direction. - // - // We separate the Plus operator into the origin point and directional part - // d* = Plus_d(d, delta_d) - // o* = Plus_o(o, d, delta_o) - // - // The direction update function Plus_d is the same as for the homogeneous - // vector parameterization: - // - // d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T - // - // where H is the householder matrix - // H_{v} = I - (2 / |v|^2) v v^T - // and - // v(d) = d - sign(d_n) |d| e_n. - // - // The origin point update function Plus_o is defined as - // - // o* = o + H_{v(d)} [0.5 delta_o, 0]^T. - - static constexpr int kDim = AmbientSpaceDimension; - using AmbientVector = Eigen::Matrix<double, kDim, 1>; - using AmbientVectorRef = Eigen::Map<Eigen::Matrix<double, kDim, 1>>; - using ConstAmbientVectorRef = - Eigen::Map<const Eigen::Matrix<double, kDim, 1>>; - using ConstTangentVectorRef = - Eigen::Map<const Eigen::Matrix<double, kDim - 1, 1>>; - - ConstAmbientVectorRef o(x_ptr); - ConstAmbientVectorRef d(x_ptr + kDim); - - ConstTangentVectorRef delta_o(delta_ptr); - ConstTangentVectorRef delta_d(delta_ptr + kDim - 1); - AmbientVectorRef o_plus_delta(x_plus_delta_ptr); - AmbientVectorRef d_plus_delta(x_plus_delta_ptr + kDim); - - const double norm_delta_d = delta_d.norm(); - - o_plus_delta = o; - - // Shortcut for zero delta direction. - if (norm_delta_d == 0.0) { - d_plus_delta = d; - - if (delta_o.isZero(0.0)) { - return true; - } - } - - // Calculate the householder transformation which is needed for f_d and f_o. - AmbientVector v; - double beta; - - // NOTE: The explicit template arguments are needed here because - // ComputeHouseholderVector is templated and some versions of MSVC - // have trouble deducing the type of v automatically. - internal::ComputeHouseholderVector<ConstAmbientVectorRef, double, kDim>( - d, &v, &beta); - - if (norm_delta_d != 0.0) { - // Map the delta from the minimum representation to the over parameterized - // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman - // (2nd Edition) for a detailed description. Note there is a typo on Page - // 625, line 4 so check the book errata. - const double norm_delta_div_2 = 0.5 * norm_delta_d; - const double sin_delta_by_delta = - std::sin(norm_delta_div_2) / norm_delta_div_2; - - // Apply the delta update to remain on the unit sphere. See section A6.9.3 - // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed - // description. - AmbientVector y; - y.template head<kDim - 1>() = 0.5 * sin_delta_by_delta * delta_d; - y[kDim - 1] = std::cos(norm_delta_div_2); - - d_plus_delta = d.norm() * (y - v * (beta * (v.transpose() * y))); - } - - // The null space is in the direction of the line, so the tangent space is - // perpendicular to the line direction. This is achieved by using the - // householder matrix of the direction and allow only movements - // perpendicular to e_n. - // - // The factor of 0.5 is used to be consistent with the line direction - // update. - AmbientVector y; - y << 0.5 * delta_o, 0; - o_plus_delta += y - v * (beta * (v.transpose() * y)); - - return true; -} - -template <int AmbientSpaceDimension> -bool LineParameterization<AmbientSpaceDimension>::ComputeJacobian( - const double* x_ptr, double* jacobian_ptr) const { - static constexpr int kDim = AmbientSpaceDimension; - using AmbientVector = Eigen::Matrix<double, kDim, 1>; - using ConstAmbientVectorRef = - Eigen::Map<const Eigen::Matrix<double, kDim, 1>>; - using MatrixRef = Eigen::Map< - Eigen::Matrix<double, 2 * kDim, 2 * (kDim - 1), Eigen::RowMajor>>; - - ConstAmbientVectorRef d(x_ptr + kDim); - MatrixRef jacobian(jacobian_ptr); - - // Clear the Jacobian as only half of the matrix is not zero. - jacobian.setZero(); - - AmbientVector v; - double beta; - - // NOTE: The explicit template arguments are needed here because - // ComputeHouseholderVector is templated and some versions of MSVC - // have trouble deducing the type of v automatically. - internal::ComputeHouseholderVector<ConstAmbientVectorRef, double, kDim>( - d, &v, &beta); - - // The Jacobian is equal to J = 0.5 * H.leftCols(kDim - 1) where H is - // the Householder matrix (H = I - beta * v * v') for the origin point. For - // the line direction part the Jacobian is scaled by the norm of the - // direction. - for (int i = 0; i < kDim - 1; ++i) { - jacobian.block(0, i, kDim, 1) = -0.5 * beta * v(i) * v; - jacobian.col(i)(i) += 0.5; - } - - jacobian.template block<kDim, kDim - 1>(kDim, kDim - 1) = - jacobian.template block<kDim, kDim - 1>(0, 0) * d.norm(); - return true; -} - -} // namespace ceres - -#endif // CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_
diff --git a/internal/ceres/autodiff_benchmarks/photometric_error.h b/internal/ceres/autodiff_benchmarks/photometric_error.h index 8ed278d..6572267 100644 --- a/internal/ceres/autodiff_benchmarks/photometric_error.h +++ b/internal/ceres/autodiff_benchmarks/photometric_error.h
@@ -165,9 +165,8 @@ if (!Project(uv, Eigen::Matrix<T, 3, 1>(p_target_scaled.col(i)))) { // If any point of the patch is outside the domain of the projection // function, the residual cannot be evaluated. For the benchmark we want - // to avoid this case and thus throw an exception to indicate - // immediately if it does actually happen after possible future changes. - throw std::runtime_error("Benchmark data leads to invalid projection."); + // to avoid this case and thus return false; + return false; } // Mind the order of u and v: Evaluate takes (row, column), but u is
diff --git a/internal/ceres/conjugate_gradients_solver.h b/internal/ceres/conjugate_gradients_solver.h index fb343ea..79fca51 100644 --- a/internal/ceres/conjugate_gradients_solver.h +++ b/internal/ceres/conjugate_gradients_solver.h
@@ -53,7 +53,7 @@ template <typename DenseVectorType> class ConjugateGradientsLinearOperator { public: - ~ConjugateGradientsLinearOperator() = default; + virtual ~ConjugateGradientsLinearOperator() = default; virtual void RightMultiplyAndAccumulate(const DenseVectorType& x, DenseVectorType& y) = 0; }; @@ -64,7 +64,7 @@ public: LinearOperatorAdapter(LinearOperator& linear_operator) : linear_operator_(linear_operator) {} - + ~LinearOperatorAdapter = default; void RightMultiplyAndAccumulate(const Vector& x, Vector& y) final { linear_operator_.RightMultiplyAndAccumulate(x, y); }
diff --git a/internal/ceres/dense_jacobian_writer.h b/internal/ceres/dense_jacobian_writer.h index d0f2c89..f25840b 100644 --- a/internal/ceres/dense_jacobian_writer.h +++ b/internal/ceres/dense_jacobian_writer.h
@@ -37,6 +37,7 @@ #include "ceres/casts.h" #include "ceres/dense_sparse_matrix.h" +#include "ceres/evaluator.h" #include "ceres/internal/disable_warnings.h" #include "ceres/internal/eigen.h" #include "ceres/internal/export.h"
diff --git a/internal/ceres/program_evaluator.h b/internal/ceres/program_evaluator.h index 366d1bf..da225dc 100644 --- a/internal/ceres/program_evaluator.h +++ b/internal/ceres/program_evaluator.h
@@ -102,6 +102,7 @@ #include "ceres/program.h" #include "ceres/residual_block.h" #include "ceres/small_blas.h" +#include "ceres/sparse_matrix.h" namespace ceres { namespace internal {