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 {
