Remove ceres::LocalParameterization Change-Id: I3bdf2f6a8857db10c984024a27f490eefd23fefa
diff --git a/bazel/ceres.bzl b/bazel/ceres.bzl index f293424..0fa2d43 100644 --- a/bazel/ceres.bzl +++ b/bazel/ceres.bzl
@@ -91,7 +91,6 @@ "linear_least_squares_problems.cc", "linear_operator.cc", "linear_solver.cc", - "local_parameterization.cc", "loss_function.cc", "low_rank_inverse_hessian.cc", "manifold.cc",
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h deleted file mode 100644 index 5f9b04d..0000000 --- a/include/ceres/autodiff_local_parameterization.h +++ /dev/null
@@ -1,158 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 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: sergey.vfx@gmail.com (Sergey Sharybin) -// mierle@gmail.com (Keir Mierle) -// sameeragarwal@google.com (Sameer Agarwal) - -#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ -#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ - -#include <memory> - -#include "ceres/internal/autodiff.h" -#include "ceres/local_parameterization.h" - -namespace ceres { - -// WARNING: LocalParameterizations are deprecated, so is -// AutoDiffLocalParameterization. They will be removed from Ceres Solver in -// version 2.2.0. Please use Manifolds and AutoDiffManifold instead. - -// Create local parameterization with Jacobians computed via automatic -// differentiation. For more information on local parameterizations, -// see include/ceres/local_parameterization.h -// -// To get an auto differentiated local parameterization, you must define -// a class with a templated operator() (a functor) that computes -// -// x_plus_delta = Plus(x, delta); -// -// the template parameter T. The autodiff framework substitutes appropriate -// "Jet" objects for T in order to compute the derivative when necessary, but -// this is hidden, and you should write the function as if T were a scalar type -// (e.g. a double-precision floating point number). -// -// The function must write the computed value in the last argument (the only -// non-const one) and return true to indicate success. -// -// For example, Quaternions have a three dimensional local -// parameterization. It's plus operation can be implemented as (taken -// from internal/ceres/auto_diff_local_parameterization_test.cc) -// -// struct QuaternionPlus { -// template<typename T> -// bool operator()(const T* x, const T* delta, T* x_plus_delta) const { -// const T squared_norm_delta = -// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; -// -// T q_delta[4]; -// if (squared_norm_delta > T(0.0)) { -// T norm_delta = sqrt(squared_norm_delta); -// const T sin_delta_by_delta = sin(norm_delta) / norm_delta; -// q_delta[0] = cos(norm_delta); -// q_delta[1] = sin_delta_by_delta * delta[0]; -// q_delta[2] = sin_delta_by_delta * delta[1]; -// q_delta[3] = sin_delta_by_delta * delta[2]; -// } else { -// // We do not just use q_delta = [1,0,0,0] here because that is a -// // constant and when used for automatic differentiation will -// // lead to a zero derivative. Instead we take a first order -// // approximation and evaluate it at zero. -// q_delta[0] = T(1.0); -// q_delta[1] = delta[0]; -// q_delta[2] = delta[1]; -// q_delta[3] = delta[2]; -// } -// -// QuaternionProduct(q_delta, x, x_plus_delta); -// return true; -// } -// }; -// -// Then given this struct, the auto differentiated local -// parameterization can now be constructed as -// -// LocalParameterization* local_parameterization = -// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>; -// | | -// Global Size ---------------+ | -// Local Size -------------------+ -// -// WARNING: Since the functor will get instantiated with different types for -// T, you must to convert from other numeric types to T before mixing -// computations with other variables of type T. In the example above, this is -// seen where instead of using k_ directly, k_ is wrapped with T(k_). - -template <typename Functor, int kGlobalSize, int kLocalSize> -class CERES_DEPRECATED_WITH_MSG("Use AutoDiffManifold instead.") - AutoDiffLocalParameterization : public LocalParameterization { - public: - AutoDiffLocalParameterization() : functor_(new Functor()) {} - - // Takes ownership of functor. - explicit AutoDiffLocalParameterization(Functor* functor) - : functor_(functor) {} - - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override { - return (*functor_)(x, delta, x_plus_delta); - } - - bool ComputeJacobian(const double* x, double* jacobian) const override { - double zero_delta[kLocalSize]; - for (int i = 0; i < kLocalSize; ++i) { - zero_delta[i] = 0.0; - } - - double x_plus_delta[kGlobalSize]; - for (int i = 0; i < kGlobalSize; ++i) { - x_plus_delta[i] = 0.0; - } - - const double* parameter_ptrs[2] = {x, zero_delta}; - double* jacobian_ptrs[2] = {nullptr, jacobian}; - return internal::AutoDifferentiate< - kGlobalSize, - internal::StaticParameterDims<kGlobalSize, kLocalSize>>( - *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); - } - - int GlobalSize() const override { return kGlobalSize; } - int LocalSize() const override { return kLocalSize; } - - const Functor& functor() const { return *functor_; } - - private: - std::unique_ptr<Functor> functor_; -}; - -} // namespace ceres - -#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h index c32477d..ba8ca35 100644 --- a/include/ceres/ceres.h +++ b/include/ceres/ceres.h
@@ -36,7 +36,6 @@ #include "ceres/autodiff_cost_function.h" #include "ceres/autodiff_first_order_function.h" -#include "ceres/autodiff_local_parameterization.h" #include "ceres/autodiff_manifold.h" #include "ceres/conditioned_cost_function.h" #include "ceres/context.h" @@ -56,7 +55,6 @@ #include "ceres/iteration_callback.h" #include "ceres/jet.h" #include "ceres/line_manifold.h" -#include "ceres/local_parameterization.h" #include "ceres/loss_function.h" #include "ceres/manifold.h" #include "ceres/numeric_diff_cost_function.h"
diff --git a/include/ceres/gradient_checker.h b/include/ceres/gradient_checker.h index 178fa2b..f12b4bb 100644 --- a/include/ceres/gradient_checker.h +++ b/include/ceres/gradient_checker.h
@@ -44,7 +44,6 @@ #include "ceres/internal/eigen.h" #include "ceres/internal/export.h" #include "ceres/internal/fixed_array.h" -#include "ceres/local_parameterization.h" #include "ceres/manifold.h" #include "glog/logging.h" @@ -59,37 +58,15 @@ // ------------------------------------ < relative_precision // max(J_actual(i, j), J_numeric(i, j)) // -// where J_actual(i, j) is the jacobian as computed by the supplied cost -// function (by the user) multiplied by the local parameterization Jacobian -// and J_numeric is the jacobian as computed by finite differences, multiplied -// by the local parameterization Jacobian as well. +// where J_actual(i, j) is the Jacobian as computed by the supplied cost +// function (by the user) multiplied by the manifold Jacobian and J_numeric is +// the Jacobian as computed by finite differences, multiplied by the manifold +// Jacobian as well. // // How to use: Fill in an array of pointers to parameter blocks for your // CostFunction, and then call Probe(). Check that the return value is 'true'. class CERES_EXPORT GradientChecker { public: - // This constructor will not take ownership of the cost function or local - // parameterizations. - // - // function: The cost function to probe. - // - // local_parameterizations: A vector of local parameterizations, one for each - // parameter block. May be nullptr or contain nullptrs to indicate that the - // respective parameter does not have a local parameterization. - // - // options: Options to use for numerical differentiation. - // - // NOTE: This constructor is deprecated and will be removed in the next public - // release of Ceres Solver. Please transition to using the Manifold based - // version. - CERES_DEPRECATED_WITH_MSG( - "Local Parameterizations are deprecated. Use the constructor that uses " - "Manifolds instead.") - GradientChecker( - const CostFunction* function, - const std::vector<const LocalParameterization*>* local_parameterizations, - const NumericDiffOptions& options); - // This will not take ownership of the cost function or manifolds. // // function: The cost function to probe. @@ -102,7 +79,6 @@ GradientChecker(const CostFunction* function, const std::vector<const Manifold*>* manifolds, const NumericDiffOptions& options); - ~GradientChecker(); // Contains results from a call to Probe for later inspection. struct CERES_EXPORT ProbeResults { @@ -166,17 +142,6 @@ GradientChecker(const GradientChecker&) = delete; void operator=(const GradientChecker&) = delete; - // This bool is used to determine whether the constructor with the - // LocalParameterizations is called or the one with Manifolds is called. If - // the former, then the vector of manifolds is a vector of ManifoldAdapter - // objects which we own and should be deleted. If the latter then they are - // real Manifold objects owned by the caller and will not be deleted. - // - // This bool is only needed during the LocalParameterization to Manifold - // transition, once this transition is complete the LocalParameterization - // based constructor and this bool will be removed. - const bool delete_manifolds_ = false; - std::vector<const Manifold*> manifolds_; const CostFunction* function_; std::unique_ptr<CostFunction> finite_diff_cost_function_;
diff --git a/include/ceres/gradient_problem.h b/include/ceres/gradient_problem.h index 6a36bea..3c14d80 100644 --- a/include/ceres/gradient_problem.h +++ b/include/ceres/gradient_problem.h
@@ -36,7 +36,6 @@ #include "ceres/first_order_function.h" #include "ceres/internal/disable_warnings.h" #include "ceres/internal/export.h" -#include "ceres/local_parameterization.h" #include "ceres/manifold.h" namespace ceres { @@ -90,47 +89,19 @@ // }; // // ceres::GradientProblem problem(new Rosenbrock()); -// -// NOTE: We are currently in the process of transitioning from -// LocalParameterization to Manifolds in the Ceres API. During this period, -// GradientProblem will support using both Manifold and LocalParameterization -// objects interchangeably. For methods in the API affected by this change, see -// their documentation below. class CERES_EXPORT GradientProblem { public: // Takes ownership of the function. explicit GradientProblem(FirstOrderFunction* function); - // Takes ownership of the function and the parameterization. - // - // NOTE: This constructor is deprecated and will be removed in the next public - // release of Ceres Solver. Please move to using the Manifold based - // constructor. - CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations are deprecated. Please use the constructor that " - "uses Manifold instead.") - GradientProblem(FirstOrderFunction* function, - LocalParameterization* parameterization); - // Takes ownership of the function and the manifold. GradientProblem(FirstOrderFunction* function, Manifold* manifold); int NumParameters() const; // Dimension of the manifold (and its tangent space). - // - // During the transition from LocalParameterization to Manifold, this method - // reports the LocalSize of the LocalParameterization or the TangentSize of - // the Manifold object associated with this problem. int NumTangentParameters() const; - // Dimension of the manifold (and its tangent space). - // - // NOTE: This method is deprecated and will be removed in the next public - // release of Ceres Solver. Please move to using NumTangentParameters() - // instead. - int NumLocalParameters() const { return NumTangentParameters(); } - // This call is not thread safe. bool Evaluate(const double* parameters, double* cost, double* gradient) const; bool Plus(const double* x, const double* delta, double* x_plus_delta) const; @@ -138,42 +109,11 @@ const FirstOrderFunction* function() const { return function_.get(); } FirstOrderFunction* mutable_function() { return function_.get(); } - // NOTE: During the transition from LocalParameterization to Manifold we need - // to support both The LocalParameterization and Manifold based constructors. - // - // When the user uses the LocalParameterization, internally the solver will - // wrap it in a ManifoldAdapter object and return it when manifold or - // mutable_manifold are called. - // - // As a result this method will return a non-nullptr result if a Manifold or a - // LocalParameterization was used when constructing the GradientProblem. const Manifold* manifold() const { return manifold_.get(); } Manifold* mutable_manifold() { return manifold_.get(); } - // If the problem is constructed without a LocalParameterization or with a - // Manifold this method will return a nullptr. - // - // NOTE: This method is deprecated and will be removed in the next public - // release of Ceres Solver. - CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.") - const LocalParameterization* parameterization() const { - return parameterization_.get(); - } - - // If the problem is constructed without a LocalParameterization or with a - // Manifold this method will return a nullptr. - // - // NOTE: This method is deprecated and will be removed in the next public - // release of Ceres Solver. - CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.") - LocalParameterization* mutable_parameterization() { - return parameterization_.get(); - } - private: std::unique_ptr<FirstOrderFunction> function_; - CERES_DEPRECATED_WITH_MSG("") - std::unique_ptr<LocalParameterization> parameterization_; std::unique_ptr<Manifold> manifold_; std::unique_ptr<double[]> scratch_; };
diff --git a/include/ceres/gradient_problem_solver.h b/include/ceres/gradient_problem_solver.h index b6290c8..5c76ad1 100644 --- a/include/ceres/gradient_problem_solver.h +++ b/include/ceres/gradient_problem_solver.h
@@ -306,10 +306,6 @@ int num_parameters = -1; // Dimension of the tangent space of the problem. - CERES_DEPRECATED_WITH_MSG("Use num_tangent_parameters.") - int num_local_parameters = -1; - - // Dimension of the tangent space of the problem. int num_tangent_parameters = -1; // Type of line search direction used.
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h deleted file mode 100644 index 5815dd1..0000000 --- a/include/ceres/local_parameterization.h +++ /dev/null
@@ -1,371 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 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: keir@google.com (Keir Mierle) -// sameeragarwal@google.com (Sameer Agarwal) - -#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ -#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ - -#include <array> -#include <memory> -#include <vector> - -#include "ceres/internal/disable_warnings.h" -#include "ceres/internal/export.h" -#include "ceres/internal/port.h" - -namespace ceres { - -// WARNING: LocalParameterizations are deprecated. They will be removed from -// Ceres Solver in version 2.2.0. Please use Manifolds instead. - -// Purpose: Sometimes parameter blocks x can overparameterize a problem -// -// min f(x) -// x -// -// In that case it is desirable to choose a parameterization for the -// block itself to remove the null directions of the cost. More -// generally, if x lies on a manifold of a smaller dimension than the -// ambient space that it is embedded in, then it is numerically and -// computationally more effective to optimize it using a -// parameterization that lives in the tangent space of that manifold -// at each point. -// -// For example, a sphere in three dimensions is a 2 dimensional -// manifold, embedded in a three dimensional space. At each point on -// the sphere, the plane tangent to it defines a two dimensional -// tangent space. For a cost function defined on this sphere, given a -// point x, moving in the direction normal to the sphere at that point -// is not useful. Thus a better way to do a local optimization is to -// optimize over two dimensional vector delta in the tangent space at -// that point and then "move" to the point x + delta, where the move -// operation involves projecting back onto the sphere. Doing so -// removes a redundant dimension from the optimization, making it -// numerically more robust and efficient. -// -// More generally we can define a function -// -// x_plus_delta = Plus(x, delta), -// -// where x_plus_delta has the same size as x, and delta is of size -// less than or equal to x. The function Plus, generalizes the -// definition of vector addition. Thus it satisfies the identify -// -// Plus(x, 0) = x, for all x. -// -// A trivial version of Plus is when delta is of the same size as x -// and -// -// Plus(x, delta) = x + delta -// -// A more interesting case if x is two dimensional vector, and the -// user wishes to hold the first coordinate constant. Then, delta is a -// scalar and Plus is defined as -// -// Plus(x, delta) = x + [0] * delta -// [1] -// -// An example that occurs commonly in Structure from Motion problems -// is when camera rotations are parameterized using Quaternion. There, -// it is useful to only make updates orthogonal to that 4-vector -// defining the quaternion. One way to do this is to let delta be a 3 -// dimensional vector and define Plus to be -// -// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x -// -// The multiplication between the two 4-vectors on the RHS is the -// standard quaternion product. -// -// Given f and a point x, optimizing f can now be restated as -// -// min f(Plus(x, delta)) -// delta -// -// Given a solution delta to this problem, the optimal value is then -// given by -// -// x* = Plus(x, delta) -// -// The class LocalParameterization defines the function Plus and its -// Jacobian which is needed to compute the Jacobian of f w.r.t delta. -class CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations will be removed from the Ceres Solver API in " - "version 2.2.0. Use Manifolds instead.") - CERES_EXPORT LocalParameterization { - public: - virtual ~LocalParameterization(); - - // Generalization of the addition operation, - // - // x_plus_delta = Plus(x, delta) - // - // with the condition that Plus(x, 0) = x. - // - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const = 0; - - // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. - // - // jacobian is a row-major GlobalSize() x LocalSize() matrix. - virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; - - // local_matrix = global_matrix * jacobian - // - // global_matrix is a num_rows x GlobalSize row major matrix. - // local_matrix is a num_rows x LocalSize row major matrix. - // jacobian(x) is the matrix returned by ComputeJacobian at x. - // - // This is only used by GradientProblem. For most normal uses, it is - // okay to use the default implementation. - virtual bool MultiplyByJacobian(const double* x, - const int num_rows, - const double* global_matrix, - double* local_matrix) const; - - // Size of x. - virtual int GlobalSize() const = 0; - - // Size of delta. - virtual int LocalSize() const = 0; -}; - -// Some basic parameterizations - -// Identity Parameterization: Plus(x, delta) = x + delta -class CERES_DEPRECATED_WITH_MSG("Use EuclideanManifold instead.") - CERES_EXPORT IdentityParameterization : public LocalParameterization { - public: - explicit IdentityParameterization(int size); - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - bool MultiplyByJacobian(const double* x, - const int num_cols, - const double* global_matrix, - double* local_matrix) const override; - int GlobalSize() const override { return size_; } - int LocalSize() const override { return size_; } - - private: - const int size_; -}; - -// Hold a subset of the parameters inside a parameter block constant. -class CERES_DEPRECATED_WITH_MSG("Use SubsetManifold instead.") - CERES_EXPORT SubsetParameterization : public LocalParameterization { - public: - explicit SubsetParameterization(int size, - const std::vector<int>& constant_parameters); - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - bool MultiplyByJacobian(const double* x, - const int num_cols, - const double* global_matrix, - double* local_matrix) const override; - int GlobalSize() const override { - return static_cast<int>(constancy_mask_.size()); - } - int LocalSize() const override { return local_size_; } - - private: - const int local_size_; - std::vector<char> constancy_mask_; -}; - -// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x -// with * being the quaternion multiplication operator. Here we assume -// that the first element of the quaternion vector is the real (cos -// theta) part. -class CERES_DEPRECATED_WITH_MSG("Use QuaternionManifold instead.") - CERES_EXPORT QuaternionParameterization : public LocalParameterization { - public: - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - int GlobalSize() const override { return 4; } - int LocalSize() const override { return 3; } -}; - -// Implements the quaternion local parameterization for Eigen's representation -// of the quaternion. Eigen uses a different internal memory layout for the -// elements of the quaternion than what is commonly used. Specifically, Eigen -// stores the elements in memory as [x, y, z, w] where the real part is last -// whereas it is typically stored first. Note, when creating an Eigen quaternion -// through the constructor the elements are accepted in w, x, y, z order. Since -// Ceres operates on parameter blocks which are raw double pointers this -// difference is important and requires a different parameterization. -// -// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x -// with * being the quaternion multiplication operator. -class CERES_DEPRECATED_WITH_MSG("Use EigenQuaternionManifold instead.") - CERES_EXPORT EigenQuaternionParameterization - : public ceres::LocalParameterization { - public: - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - int GlobalSize() const override { return 4; } - int LocalSize() const override { return 3; } -}; - -// This provides a parameterization for homogeneous vectors which are commonly -// used in Structure from Motion problems. One example where they are used is -// in representing points whose triangulation is ill-conditioned. Here it is -// advantageous to use an over-parameterization since homogeneous vectors can -// represent points at infinity. -// -// The plus operator is defined as -// Plus(x, delta) = -// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x -// -// with * defined as an operator which applies the update orthogonal to x to -// remain on the sphere. We assume that the last element of x is the scalar -// component. The size of the homogeneous vector is required to be greater than -// 1. -class CERES_DEPRECATED_WITH_MSG("Use SphereManifold instead.") CERES_EXPORT - HomogeneousVectorParameterization : public LocalParameterization { - public: - explicit HomogeneousVectorParameterization(int size); - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - int GlobalSize() const override { return size_; } - int LocalSize() const override { return size_ - 1; } - - private: - const int size_; -}; - -// This provides a parameterization for lines, where the line is -// over-parameterized by an origin point and a direction vector. So the -// parameter vector size needs to be two times the ambient space dimension, -// where the first half is interpreted as the origin point and the second half -// as the direction. -// -// The plus operator for the line direction is the same as for the -// HomogeneousVectorParameterization. The update of the origin point is -// perpendicular to the line direction before the update. -// -// This local parameterization is a special case of the affine Grassmannian -// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) -// for the case Graff_1(R^n). -template <int AmbientSpaceDimension> -class CERES_DEPRECATED_WITH_MSG("Use LineManifold instead.") - LineParameterization : public LocalParameterization { - public: - static_assert(AmbientSpaceDimension >= 2, - "The ambient space must be at least 2"); - - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - int GlobalSize() const override { return 2 * AmbientSpaceDimension; } - int LocalSize() const override { return 2 * (AmbientSpaceDimension - 1); } -}; - -// Construct a local parameterization by taking the Cartesian product -// of a number of other local parameterizations. This is useful, when -// a parameter block is the cartesian product of two or more -// manifolds. For example the parameters of a camera consist of a -// rotation and a translation, i.e., SO(3) x R^3. -// -// Example usage: -// -// ProductParameterization product_param(new QuaterionionParameterization(), -// new IdentityParameterization(3)); -// -// is the local parameterization for a rigid transformation, where the -// rotation is represented using a quaternion. -// -class CERES_DEPRECATED_WITH_MSG("Use ProductManifold instead.") - CERES_EXPORT ProductParameterization : public LocalParameterization { - public: - ProductParameterization(const ProductParameterization&) = delete; - ProductParameterization& operator=(const ProductParameterization&) = delete; - // - // NOTE: The constructor takes ownership of the input local - // parameterizations. - // - template <typename... LocalParams> - explicit ProductParameterization(LocalParams*... local_params) - : local_params_(sizeof...(LocalParams)) { - constexpr int kNumLocalParams = sizeof...(LocalParams); - static_assert(kNumLocalParams >= 2, - "At least two local parameterizations must be specified."); - - using LocalParameterizationPtr = std::unique_ptr<LocalParameterization>; - - // Wrap all raw pointers into std::unique_ptr for exception safety. - std::array<LocalParameterizationPtr, kNumLocalParams> local_params_array{ - LocalParameterizationPtr(local_params)...}; - - // Initialize internal state. - for (int i = 0; i < kNumLocalParams; ++i) { - LocalParameterizationPtr& param = local_params_[i]; - param = std::move(local_params_array[i]); - - buffer_size_ = - std::max(buffer_size_, param->LocalSize() * param->GlobalSize()); - global_size_ += param->GlobalSize(); - local_size_ += param->LocalSize(); - } - } - - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override; - bool ComputeJacobian(const double* x, double* jacobian) const override; - int GlobalSize() const override { return global_size_; } - int LocalSize() const override { return local_size_; } - - private: - std::vector<std::unique_ptr<LocalParameterization>> local_params_; - int local_size_{0}; - int global_size_{0}; - int buffer_size_{0}; -}; - -} // namespace ceres - -// clang-format off -#include "ceres/internal/reenable_warnings.h" -// clang-format on - -#include "ceres/internal/line_parameterization.h" - -#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_
diff --git a/include/ceres/problem.h b/include/ceres/problem.h index cbb1dff..7219699 100644 --- a/include/ceres/problem.h +++ b/include/ceres/problem.h
@@ -118,13 +118,6 @@ // problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x3); // // Please see cost_function.h for details of the CostFunction object. -// -// NOTE: We are currently in the process of transitioning from -// LocalParameterization to Manifolds in the Ceres API. During this period, -// Problem will support using both Manifold and LocalParameterization objects -// interchangeably. In particular, adding a LocalParameterization to a parameter -// block is the same as adding a Manifold to that parameter block. For methods -// in the API affected by this change, see their documentation below. class CERES_EXPORT Problem { public: struct CERES_EXPORT Options { @@ -137,10 +130,6 @@ // the pointers only once, since sharing objects is allowed. Ownership cost_function_ownership = TAKE_OWNERSHIP; Ownership loss_function_ownership = TAKE_OWNERSHIP; - CERES_DEPRECATED_WITH_MSG( - "Local Parameterizations are deprecated. Use Manifold and " - "manifold_ownership instead.") - Ownership local_parameterization_ownership = TAKE_OWNERSHIP; Ownership manifold_ownership = TAKE_OWNERSHIP; // If true, trades memory for faster RemoveResidualBlock() and @@ -271,38 +260,6 @@ // pointer but a different size will result in a crash. void AddParameterBlock(double* values, int size); - // Add a parameter block with appropriate size and parameterization to the - // problem. It is okay for local_parameterization to be nullptr. - // - // Repeated calls with the same arguments are ignored. Repeated calls - // with the same double pointer but a different size results in a crash - // (unless Solver::Options::disable_all_safety_checks is set to true). - // - // Repeated calls with the same double pointer and size but different - // LocalParameterization is equivalent to calling - // SetParameterization(local_parameterization), i.e., any previously - // associated LocalParameterization or Manifold object will be replaced with - // the local_parameterization. - // - // NOTE: - // ---- - // - // This method is deprecated and will be removed in the next public - // release of Ceres Solver. Please move to using the Manifold based version of - // AddParameterBlock. - // - // During the transition from LocalParameterization to Manifold, internally - // the LocalParameterization is treated as a Manifold by wrapping it using a - // ManifoldAdapter object. So HasManifold() will return true, GetManifold() - // will return the wrapped object and ParameterBlockTangentSize() will return - // the LocalSize of the LocalParameterization. - CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations are deprecated. Use the version with Manifolds " - "instead.") - void AddParameterBlock(double* values, - int size, - LocalParameterization* local_parameterization); - // Add a parameter block with appropriate size and Manifold to the // problem. It is okay for manifold to be nullptr. // @@ -314,16 +271,6 @@ // is equivalent to calling SetManifold(manifold), i.e., any previously // associated LocalParameterization or Manifold object will be replaced with // the manifold. - // - // Note: - // ---- - // - // During the transition from LocalParameterization to Manifold, calling - // AddParameterBlock with a Manifold when a LocalParameterization is already - // associated with the parameter block is okay. It is equivalent to calling - // SetManifold(manifold), i.e., any previously associated - // LocalParameterization or Manifold object will be replaced with the - // manifold. void AddParameterBlock(double* values, int size, Manifold* manifold); // Remove a parameter block from the problem. The LocalParameterization or @@ -365,66 +312,6 @@ // Manifold with a zero dimensional tangent space with it. bool IsParameterBlockConstant(const double* values) const; - // Set the LocalParameterization for the parameter block. Calling - // SetParameterization with nullptr will clear any previously set - // LocalParameterization or Manifold for the parameter block. - // - // Repeated calls will cause any previously associated LocalParameterization - // or Manifold object to be replaced with the local_parameterization. - // - // The local_parameterization is owned by the Problem by default (See - // Problem::Options to override this behaviour). - // - // It is acceptable to set the same LocalParameterization for multiple - // parameter blocks; the destructor is careful to delete - // LocalParameterizations only once. - // - // NOTE: - // ---- - // - // This method is deprecated and will be removed in the next public - // release of Ceres Solver. Please move to using the SetManifold instead. - // - // During the transition from LocalParameterization to Manifold, internally - // the LocalParameterization is treated as a Manifold by wrapping it using a - // ManifoldAdapter object. So HasManifold() will return true, GetManifold() - // will return the wrapped object and ParameterBlockTangentSize will return - // the same value of ParameterBlockLocalSize. - CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations are deprecated. Use SetManifold instead.") - void SetParameterization(double* values, - LocalParameterization* local_parameterization); - - // Get the LocalParameterization object associated with this parameter block. - // If there is no LocalParameterization associated then nullptr is returned. - // - // NOTE: This method is deprecated and will be removed in the next public - // release of Ceres Solver. Use GetManifold instead. - // - // Note also that if a LocalParameterization is associated with a parameter - // block, HasManifold will return true and GetManifold will return the - // LocalParameterization wrapped in a ManifoldAdapter. - // - // The converse is NOT true, i.e., if a Manifold is associated with a - // parameter block, HasParameterization will return false and - // GetParameterization will return a nullptr. - CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations are deprecated. Use GetManifold " - "instead.") - const LocalParameterization* GetParameterization(const double* values) const; - - // Returns true if a LocalParameterization is associated with this parameter - // block, false otherwise. - // - // NOTE: This method is deprecated and will be removed in the next public - // release of Ceres Solver. Use HasManifold instead. - // - // Note also that if a Manifold is associated with the parameter block, this - // method will return false. - CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations are deprecated. Use HasManifold instead.") - bool HasParameterization(const double* values) const; - // Set the Manifold for the parameter block. Calling SetManifold with nullptr // will clear any previously set LocalParameterization or Manifold for the // parameter block. @@ -440,14 +327,7 @@ // Get the Manifold object associated with this parameter block. // - // If there is no Manifold Or LocalParameterization object associated then - // nullptr is returned. - // - // NOTE: During the transition from LocalParameterization to Manifold, - // internally the LocalParameterization is treated as a Manifold by wrapping - // it using a ManifoldAdapter object. So calling GetManifold on a parameter - // block with a LocalParameterization associated with it will return the - // LocalParameterization wrapped in a ManifoldAdapter + // If there is no Manifold object associated then nullptr is returned. const Manifold* GetManifold(const double* values) const; // Returns true if a Manifold or a LocalParameterization is associated with @@ -486,15 +366,6 @@ // The dimension of the tangent space of the LocalParameterization or Manifold // for the parameter block. If there is no LocalParameterization or Manifold - // associated with this parameter block, then ParameterBlockLocalSize = - // ParameterBlockSize. - CERES_DEPRECATED_WITH_MSG( - "LocalParameterizations are deprecated. Use ParameterBlockTangentSize " - "instead.") - int ParameterBlockLocalSize(const double* values) const; - - // The dimension of the tangent space of the LocalParameterization or Manifold - // for the parameter block. If there is no LocalParameterization or Manifold // associated with this parameter block, then ParameterBlockTangentSize = // ParameterBlockSize. int ParameterBlockTangentSize(const double* values) const;
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index f533f61..75df3c6 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -59,7 +59,6 @@ c_api.cc gradient_checker.cc gradient_problem.cc - local_parameterization.cc loss_function.cc manifold.cc normal_prior.cc @@ -336,7 +335,7 @@ set_target_properties(ceres PROPERTIES VERSION ${CERES_VERSION} - SOVERSION 3) + SOVERSION 4) target_link_libraries(ceres PUBLIC ${CERES_LIBRARY_PUBLIC_DEPENDENCIES} @@ -464,7 +463,6 @@ ceres_test(autodiff) ceres_test(autodiff_first_order_function) ceres_test(autodiff_cost_function) - ceres_test(autodiff_local_parameterization) ceres_test(autodiff_manifold) ceres_test(block_jacobi_preconditioner) ceres_test(block_random_access_dense_matrix) @@ -517,7 +515,6 @@ ceres_test(levenberg_marquardt_strategy) ceres_test(line_search_minimizer) ceres_test(line_search_preprocessor) - ceres_test(local_parameterization) ceres_test(loss_function) ceres_test(manifold) ceres_test(minimizer)
diff --git a/internal/ceres/autodiff_local_parameterization_test.cc b/internal/ceres/autodiff_local_parameterization_test.cc deleted file mode 100644 index fc49351..0000000 --- a/internal/ceres/autodiff_local_parameterization_test.cc +++ /dev/null
@@ -1,225 +0,0 @@ -// 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) - -#include "ceres/autodiff_local_parameterization.h" - -#include <cmath> - -#include "ceres/local_parameterization.h" -#include "ceres/rotation.h" -#include "gtest/gtest.h" - -namespace ceres::internal { - -struct IdentityPlus { - template <typename T> - bool operator()(const T* x, const T* delta, T* x_plus_delta) const { - for (int i = 0; i < 3; ++i) { - x_plus_delta[i] = x[i] + delta[i]; - } - return true; - } -}; - -TEST(AutoDiffLocalParameterizationTest, IdentityParameterization) { - AutoDiffLocalParameterization<IdentityPlus, 3, 3> parameterization; - - double x[3] = {1.0, 2.0, 3.0}; - double delta[3] = {0.0, 1.0, 2.0}; - double x_plus_delta[3] = {0.0, 0.0, 0.0}; - parameterization.Plus(x, delta, x_plus_delta); - - EXPECT_EQ(x_plus_delta[0], 1.0); - EXPECT_EQ(x_plus_delta[1], 3.0); - EXPECT_EQ(x_plus_delta[2], 5.0); - - double jacobian[9]; - parameterization.ComputeJacobian(x, jacobian); - int k = 0; - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j, ++k) { - EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); - } - } -} - -struct ScaledPlus { - explicit ScaledPlus(const double& scale_factor) - : scale_factor_(scale_factor) {} - - template <typename T> - bool operator()(const T* x, const T* delta, T* x_plus_delta) const { - for (int i = 0; i < 3; ++i) { - x_plus_delta[i] = x[i] + T(scale_factor_) * delta[i]; - } - return true; - } - - const double scale_factor_; -}; - -TEST(AutoDiffLocalParameterizationTest, ScaledParameterization) { - const double kTolerance = 1e-14; - - AutoDiffLocalParameterization<ScaledPlus, 3, 3> parameterization( - new ScaledPlus(1.2345)); - - double x[3] = {1.0, 2.0, 3.0}; - double delta[3] = {0.0, 1.0, 2.0}; - double x_plus_delta[3] = {0.0, 0.0, 0.0}; - parameterization.Plus(x, delta, x_plus_delta); - - EXPECT_NEAR(x_plus_delta[0], 1.0, kTolerance); - EXPECT_NEAR(x_plus_delta[1], 3.2345, kTolerance); - EXPECT_NEAR(x_plus_delta[2], 5.469, kTolerance); - - double jacobian[9]; - parameterization.ComputeJacobian(x, jacobian); - int k = 0; - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j, ++k) { - EXPECT_NEAR(jacobian[k], (i == j) ? 1.2345 : 0.0, kTolerance); - } - } -} - -struct QuaternionPlus { - template <typename T> - bool operator()(const T* x, const T* delta, T* x_plus_delta) const { - const T squared_norm_delta = - delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - - T q_delta[4]; - if (squared_norm_delta > T(0.0)) { - T norm_delta = sqrt(squared_norm_delta); - const T sin_delta_by_delta = sin(norm_delta) / norm_delta; - q_delta[0] = cos(norm_delta); - q_delta[1] = sin_delta_by_delta * delta[0]; - q_delta[2] = sin_delta_by_delta * delta[1]; - q_delta[3] = sin_delta_by_delta * delta[2]; - } else { - // We do not just use q_delta = [1,0,0,0] here because that is a - // constant and when used for automatic differentiation will - // lead to a zero derivative. Instead we take a first order - // approximation and evaluate it at zero. - q_delta[0] = T(1.0); - q_delta[1] = delta[0]; - q_delta[2] = delta[1]; - q_delta[3] = delta[2]; - } - - QuaternionProduct(q_delta, x, x_plus_delta); - return true; - } -}; - -static void QuaternionParameterizationTestHelper(const double* x, - const double* delta) { - const double kTolerance = 1e-14; - double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0}; - double jacobian_ref[12]; - - QuaternionParameterization ref_parameterization; - ref_parameterization.Plus(x, delta, x_plus_delta_ref); - ref_parameterization.ComputeJacobian(x, jacobian_ref); - - double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; - double jacobian[12]; - AutoDiffLocalParameterization<QuaternionPlus, 4, 3> parameterization; - parameterization.Plus(x, delta, x_plus_delta); - parameterization.ComputeJacobian(x, jacobian); - - for (int i = 0; i < 4; ++i) { - EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); - } - - // clang-format off - const double x_plus_delta_norm = - sqrt(x_plus_delta[0] * x_plus_delta[0] + - x_plus_delta[1] * x_plus_delta[1] + - x_plus_delta[2] * x_plus_delta[2] + - x_plus_delta[3] * x_plus_delta[3]); - // clang-format on - - EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); - - for (int i = 0; i < 12; ++i) { - EXPECT_TRUE(std::isfinite(jacobian[i])); - EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) - << "Jacobian mismatch: i = " << i << "\n Expected \n" - << ConstMatrixRef(jacobian_ref, 4, 3) << "\n Actual \n" - << ConstMatrixRef(jacobian, 4, 3); - } -} - -TEST(AutoDiffLocalParameterization, QuaternionParameterizationZeroTest) { - double x[4] = {0.5, 0.5, 0.5, 0.5}; - double delta[3] = {0.0, 0.0, 0.0}; - QuaternionParameterizationTestHelper(x, delta); -} - -TEST(AutoDiffLocalParameterization, QuaternionParameterizationNearZeroTest) { - double x[4] = {0.52, 0.25, 0.15, 0.45}; - // clang-format off - double norm_x = sqrt(x[0] * x[0] + - x[1] * x[1] + - x[2] * x[2] + - x[3] * x[3]); - // clang-format on - for (double& x_i : x) { - x_i = x_i / norm_x; - } - - double delta[3] = {0.24, 0.15, 0.10}; - for (double& delta_i : delta) { - delta_i = delta_i * 1e-14; - } - - QuaternionParameterizationTestHelper(x, delta); -} - -TEST(AutoDiffLocalParameterization, QuaternionParameterizationNonZeroTest) { - double x[4] = {0.52, 0.25, 0.15, 0.45}; - // clang-format off - double norm_x = sqrt(x[0] * x[0] + - x[1] * x[1] + - x[2] * x[2] + - x[3] * x[3]); - // clang-format on - - for (double& x_i : x) { - x_i = x_i / norm_x; - } - - double delta[3] = {0.24, 0.15, 0.10}; - QuaternionParameterizationTestHelper(x, delta); -} - -} // namespace ceres::internal
diff --git a/internal/ceres/covariance_test.cc b/internal/ceres/covariance_test.cc index ad9fe0a..caea18c 100644 --- a/internal/ceres/covariance_test.cc +++ b/internal/ceres/covariance_test.cc
@@ -42,7 +42,6 @@ #include "ceres/cost_function.h" #include "ceres/covariance_impl.h" #include "ceres/internal/config.h" -#include "ceres/local_parameterization.h" #include "ceres/manifold.h" #include "ceres/map_util.h" #include "ceres/problem_impl.h" @@ -378,27 +377,6 @@ } // x_plus_delta = delta * x; -class PolynomialParameterization : public LocalParameterization { - public: - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const final { - x_plus_delta[0] = delta[0] * x[0]; - x_plus_delta[1] = delta[0] * x[1]; - return true; - } - - bool ComputeJacobian(const double* x, double* jacobian) const final { - jacobian[0] = x[0]; - jacobian[1] = x[1]; - return true; - } - - int GlobalSize() const final { return 2; } - int LocalSize() const final { return 1; } -}; - -// x_plus_delta = delta * x; class PolynomialManifold : public Manifold { public: bool Plus(const double* x, @@ -766,193 +744,6 @@ ComputeAndCompareCovarianceBlocks(options, expected_covariance); } -TEST_F(CovarianceTest, LocalParameterization) { - double* x = parameters_; - double* y = x + 2; - - problem_.SetParameterization(x, new PolynomialParameterization); - - vector<int> subset; - subset.push_back(2); - problem_.SetParameterization(y, new SubsetParameterization(3, subset)); - - // Raw Jacobian: J - // - // 1 0 0 0 0 0 - // 0 1 0 0 0 0 - // 0 0 2 0 0 0 - // 0 0 0 2 0 0 - // 0 0 0 0 2 0 - // 0 0 0 0 0 5 - // -5 -6 1 2 3 0 - // 3 -2 0 0 0 2 - - // Local to global jacobian: A - // - // 1 0 0 0 - // 1 0 0 0 - // 0 1 0 0 - // 0 0 1 0 - // 0 0 0 0 - // 0 0 0 1 - - // A * inv((J*A)'*(J*A)) * A' - // Computed using octave. - // clang-format off - double expected_covariance[] = { - 0.01766, 0.01766, 0.02158, 0.04316, 0.00000, -0.00122, - 0.01766, 0.01766, 0.02158, 0.04316, 0.00000, -0.00122, - 0.02158, 0.02158, 0.24860, -0.00281, 0.00000, -0.00149, - 0.04316, 0.04316, -0.00281, 0.24439, 0.00000, -0.00298, - 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, - -0.00122, -0.00122, -0.00149, -0.00298, 0.00000, 0.03457 - }; - // clang-format on - - Covariance::Options options; - -#ifndef CERES_NO_SUITESPARSE - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = SUITE_SPARSE; - ComputeAndCompareCovarianceBlocks(options, expected_covariance); -#endif - - options.algorithm_type = DENSE_SVD; - ComputeAndCompareCovarianceBlocks(options, expected_covariance); - - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = EIGEN_SPARSE; - ComputeAndCompareCovarianceBlocks(options, expected_covariance); -} - -TEST_F(CovarianceTest, LocalParameterizationInTangentSpace) { - double* x = parameters_; - double* y = x + 2; - double* z = y + 3; - - problem_.SetParameterization(x, new PolynomialParameterization); - - vector<int> subset; - subset.push_back(2); - problem_.SetParameterization(y, new SubsetParameterization(3, subset)); - - local_column_bounds_[x] = make_pair(0, 1); - local_column_bounds_[y] = make_pair(1, 3); - local_column_bounds_[z] = make_pair(3, 4); - - // Raw Jacobian: J - // - // 1 0 0 0 0 0 - // 0 1 0 0 0 0 - // 0 0 2 0 0 0 - // 0 0 0 2 0 0 - // 0 0 0 0 2 0 - // 0 0 0 0 0 5 - // -5 -6 1 2 3 0 - // 3 -2 0 0 0 2 - - // Local to global jacobian: A - // - // 1 0 0 0 - // 1 0 0 0 - // 0 1 0 0 - // 0 0 1 0 - // 0 0 0 0 - // 0 0 0 1 - - // inv((J*A)'*(J*A)) - // Computed using octave. - // clang-format off - double expected_covariance[] = { - 0.01766, 0.02158, 0.04316, -0.00122, - 0.02158, 0.24860, -0.00281, -0.00149, - 0.04316, -0.00281, 0.24439, -0.00298, - -0.00122, -0.00149, -0.00298, 0.03457 // NOLINT - }; - // clang-format on - - Covariance::Options options; - -#ifndef CERES_NO_SUITESPARSE - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = SUITE_SPARSE; - - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); -#endif - - options.algorithm_type = DENSE_SVD; - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); - - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = EIGEN_SPARSE; - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); -} - -TEST_F(CovarianceTest, LocalParameterizationInTangentSpaceWithConstantBlocks) { - double* x = parameters_; - double* y = x + 2; - double* z = y + 3; - - problem_.SetParameterization(x, new PolynomialParameterization); - problem_.SetParameterBlockConstant(x); - - vector<int> subset; - subset.push_back(2); - problem_.SetParameterization(y, new SubsetParameterization(3, subset)); - problem_.SetParameterBlockConstant(y); - - local_column_bounds_[x] = make_pair(0, 1); - local_column_bounds_[y] = make_pair(1, 3); - local_column_bounds_[z] = make_pair(3, 4); - - // Raw Jacobian: J - // - // 1 0 0 0 0 0 - // 0 1 0 0 0 0 - // 0 0 2 0 0 0 - // 0 0 0 2 0 0 - // 0 0 0 0 2 0 - // 0 0 0 0 0 5 - // -5 -6 1 2 3 0 - // 3 -2 0 0 0 2 - - // Local to global jacobian: A - // - // 0 0 0 0 - // 0 0 0 0 - // 0 0 0 0 - // 0 0 0 0 - // 0 0 0 0 - // 0 0 0 1 - - // pinv((J*A)'*(J*A)) - // Computed using octave. - // clang-format off - double expected_covariance[] = { - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.034482 // NOLINT - }; - // clang-format on - - Covariance::Options options; - -#ifndef CERES_NO_SUITESPARSE - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = SUITE_SPARSE; - - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); -#endif - - options.algorithm_type = DENSE_SVD; - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); - - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = EIGEN_SPARSE; - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); -} - TEST_F(CovarianceTest, Manifold) { double* x = parameters_; double* y = x + 2; @@ -1251,49 +1042,6 @@ ComputeAndCompareCovarianceBlocks(options, expected_covariance); } -TEST_F( - CovarianceTest, - DenseCovarianceMatrixFromSetOfParametersInTangentSpaceUsingLocalParameterizations) { - Covariance::Options options; - Covariance covariance(options); - double* x = parameters_; - double* y = x + 2; - double* z = y + 3; - - problem_.SetParameterization(x, new PolynomialParameterization); - - vector<int> subset; - subset.push_back(2); - problem_.SetParameterization(y, new SubsetParameterization(3, subset)); - - local_column_bounds_[x] = make_pair(0, 1); - local_column_bounds_[y] = make_pair(1, 3); - local_column_bounds_[z] = make_pair(3, 4); - - vector<const double*> parameter_blocks; - parameter_blocks.push_back(x); - parameter_blocks.push_back(y); - parameter_blocks.push_back(z); - covariance.Compute(parameter_blocks, &problem_); - double expected_covariance[16]; - covariance.GetCovarianceMatrixInTangentSpace(parameter_blocks, - expected_covariance); - -#ifndef CERES_NO_SUITESPARSE - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = SUITE_SPARSE; - - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); -#endif - - options.algorithm_type = DENSE_SVD; - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); - - options.algorithm_type = SPARSE_QR; - options.sparse_linear_algebra_library_type = EIGEN_SPARSE; - ComputeAndCompareCovarianceBlocksInTangentSpace(options, expected_covariance); -} - TEST_F(CovarianceTest, DenseCovarianceMatrixFromSetOfParametersInTangentSpace) { Covariance::Options options; Covariance covariance(options); @@ -1462,74 +1210,6 @@ } }; -TEST(Covariance, ZeroSizedLocalParameterizationGetCovariance) { - double x = 0.0; - double y = 1.0; - Problem problem; - problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y); - problem.SetParameterization(&y, new SubsetParameterization(1, {0})); - // J = [-1 0] - // [ 0 0] - Covariance::Options options; - options.algorithm_type = DENSE_SVD; - Covariance covariance(options); - vector<pair<const double*, const double*>> covariance_blocks; - covariance_blocks.emplace_back(&x, &x); - covariance_blocks.emplace_back(&x, &y); - covariance_blocks.emplace_back(&y, &x); - covariance_blocks.emplace_back(&y, &y); - EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem)); - - double value = -1; - covariance.GetCovarianceBlock(&x, &x, &value); - EXPECT_NEAR(value, 1.0, std::numeric_limits<double>::epsilon()); - - value = -1; - covariance.GetCovarianceBlock(&x, &y, &value); - EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon()); - - value = -1; - covariance.GetCovarianceBlock(&y, &x, &value); - EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon()); - - value = -1; - covariance.GetCovarianceBlock(&y, &y, &value); - EXPECT_NEAR(value, 0.0, std::numeric_limits<double>::epsilon()); -} - -TEST(Covariance, ZeroSizedLocalParameterizationGetCovarianceInTangentSpace) { - double x = 0.0; - double y = 1.0; - Problem problem; - problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y); - problem.SetParameterization(&y, new SubsetParameterization(1, {0})); - // J = [-1 0] - // [ 0 0] - Covariance::Options options; - options.algorithm_type = DENSE_SVD; - Covariance covariance(options); - vector<pair<const double*, const double*>> covariance_blocks; - covariance_blocks.emplace_back(&x, &x); - covariance_blocks.emplace_back(&x, &y); - covariance_blocks.emplace_back(&y, &x); - covariance_blocks.emplace_back(&y, &y); - EXPECT_TRUE(covariance.Compute(covariance_blocks, &problem)); - - double value = -1; - covariance.GetCovarianceBlockInTangentSpace(&x, &x, &value); - EXPECT_NEAR(value, 1.0, std::numeric_limits<double>::epsilon()); - - value = -1; - // The following three calls, should not touch this value, since the - // tangent space is of size zero - covariance.GetCovarianceBlockInTangentSpace(&x, &y, &value); - EXPECT_EQ(value, -1); - covariance.GetCovarianceBlockInTangentSpace(&y, &x, &value); - EXPECT_EQ(value, -1); - covariance.GetCovarianceBlockInTangentSpace(&y, &y, &value); - EXPECT_EQ(value, -1); -} - TEST(Covariance, ZeroSizedManifoldGetCovariance) { double x = 0.0; double y = 1.0;
diff --git a/internal/ceres/evaluator_test.cc b/internal/ceres/evaluator_test.cc index 10c0d1d..24da250 100644 --- a/internal/ceres/evaluator_test.cc +++ b/internal/ceres/evaluator_test.cc
@@ -40,7 +40,6 @@ #include "ceres/crs_matrix.h" #include "ceres/evaluator_test_utils.h" #include "ceres/internal/eigen.h" -#include "ceres/local_parameterization.h" #include "ceres/manifold.h" #include "ceres/problem_impl.h" #include "ceres/program.h" @@ -395,68 +394,6 @@ CheckAllEvaluationCombinations(expected); } -TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) { - // Add the parameters in explicit order to force the ordering in the program. - problem.AddParameterBlock(x, 2); - - // Fix y's first dimension. - vector<int> y_fixed; - y_fixed.push_back(0); - problem.AddParameterBlock(y, 3, new SubsetParameterization(3, y_fixed)); - - // Fix z's second dimension. - vector<int> z_fixed; - z_fixed.push_back(1); - problem.AddParameterBlock(z, 4, new SubsetParameterization(4, z_fixed)); - - // f(x, y) in R^2 - problem.AddResidualBlock( - new ParameterIgnoringCostFunction<1, 2, 2, 3>, nullptr, x, y); - - // g(x, z) in R^3 - problem.AddResidualBlock( - new ParameterIgnoringCostFunction<2, 3, 2, 4>, nullptr, x, z); - - // h(y, z) in R^4 - problem.AddResidualBlock( - new ParameterIgnoringCostFunction<3, 4, 3, 4>, nullptr, y, z); - - // clang-format off - ExpectedEvaluation expected = { - // Rows/columns - 9, 7, - // Cost - // f g h - ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0, - // Residuals - { 1.0, 2.0, // f - 1.0, 2.0, 3.0, // g - 1.0, 2.0, 3.0, 4.0 // h - }, - // Gradient - { 15.0, 30.0, // x - 66.0, 99.0, // y - 42.0, 126.0, 168.0 // z - }, - // Jacobian - // x y z - { /* f(x, y) */ 1, 2, 2, 3, 0, 0, 0, - 1, 2, 2, 3, 0, 0, 0, - - /* g(x, z) */ 2, 4, 0, 0, 2, 6, 8, - 2, 4, 0, 0, 2, 6, 8, - 2, 4, 0, 0, 2, 6, 8, - - /* h(y, z) */ 0, 0, 6, 9, 3, 9, 12, - 0, 0, 6, 9, 3, 9, 12, - 0, 0, 6, 9, 3, 9, 12, - 0, 0, 6, 9, 3, 9, 12 - } - }; - // clang-format on - CheckAllEvaluationCombinations(expected); -} - TEST_P(EvaluatorTest, MultipleResidualsWithManifolds) { // Add the parameters in explicit order to force the ordering in the program. problem.AddParameterBlock(x, 2);
diff --git a/internal/ceres/gradient_checker.cc b/internal/ceres/gradient_checker.cc index 777001e..68bcf86 100644 --- a/internal/ceres/gradient_checker.cc +++ b/internal/ceres/gradient_checker.cc
@@ -40,7 +40,6 @@ #include <vector> #include "ceres/is_close.h" -#include "ceres/manifold_adapter.h" #include "ceres/stringprintf.h" #include "ceres/types.h" @@ -116,37 +115,6 @@ } } // namespace -GradientChecker::GradientChecker( - const CostFunction* function, - const vector<const LocalParameterization*>* local_parameterizations, - const NumericDiffOptions& options) - : delete_manifolds_(true), function_(function) { - CHECK(function != nullptr); - manifolds_.resize(function->parameter_block_sizes().size(), nullptr); - - // Wrap the local parameterization into manifold objects using - // ManifoldAdapter. - for (int i = 0; i < manifolds_.size(); ++i) { - const LocalParameterization* local_param = local_parameterizations->at(i); - if (local_param == nullptr) { - continue; - } - manifolds_[i] = new internal::ManifoldAdapter(local_param); - } - - auto finite_diff_cost_function = - std::make_unique<DynamicNumericDiffCostFunction<CostFunction, RIDDERS>>( - function, DO_NOT_TAKE_OWNERSHIP, options); - const vector<int32_t>& parameter_block_sizes = - function->parameter_block_sizes(); - for (int32_t parameter_block_size : parameter_block_sizes) { - finite_diff_cost_function->AddParameterBlock(parameter_block_size); - } - finite_diff_cost_function->SetNumResiduals(function->num_residuals()); - - finite_diff_cost_function_ = std::move(finite_diff_cost_function); -} - GradientChecker::GradientChecker(const CostFunction* function, const vector<const Manifold*>* manifolds, const NumericDiffOptions& options) @@ -172,14 +140,6 @@ finite_diff_cost_function_ = std::move(finite_diff_cost_function); } -GradientChecker::~GradientChecker() { - if (delete_manifolds_) { - for (const auto m : manifolds_) { - delete m; - } - } -} - bool GradientChecker::Probe(double const* const* parameters, double relative_precision, ProbeResults* results_param) const {
diff --git a/internal/ceres/gradient_checker_test.cc b/internal/ceres/gradient_checker_test.cc index fcc4892..aedb0b9 100644 --- a/internal/ceres/gradient_checker_test.cc +++ b/internal/ceres/gradient_checker_test.cc
@@ -352,241 +352,6 @@ ExpectArraysClose(p.size(), p.data(), q.data(), tolerance); } -// Helper local parameterization that multiplies the delta vector by the given -// jacobian and adds it to the parameter. -class MatrixParameterization : public LocalParameterization { - public: - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const final { - VectorRef(x_plus_delta, GlobalSize()) = - ConstVectorRef(x, GlobalSize()) + - (global_to_local_ * ConstVectorRef(delta, LocalSize())); - return true; - } - - bool ComputeJacobian(const double* /*x*/, double* jacobian) const final { - MatrixRef(jacobian, GlobalSize(), LocalSize()) = global_to_local_; - return true; - } - - int GlobalSize() const final { return global_to_local_.rows(); } - int LocalSize() const final { return global_to_local_.cols(); } - - Matrix global_to_local_; -}; - -TEST(GradientChecker, TestCorrectnessWithLocalParameterizations) { - // Create cost function. - Eigen::Vector3d residual_offset(100.0, 200.0, 300.0); - LinearCostFunction cost_function(residual_offset); - Eigen::Matrix<double, 3, 3, Eigen::RowMajor> j0; - j0.row(0) << 1.0, 2.0, 3.0; - j0.row(1) << 4.0, 5.0, 6.0; - j0.row(2) << 7.0, 8.0, 9.0; - Eigen::Matrix<double, 3, 2, Eigen::RowMajor> j1; - j1.row(0) << 10.0, 11.0; - j1.row(1) << 12.0, 13.0; - j1.row(2) << 14.0, 15.0; - - Eigen::Vector3d param0(1.0, 2.0, 3.0); - Eigen::Vector2d param1(4.0, 5.0); - - cost_function.AddParameter(j0); - cost_function.AddParameter(j1); - - std::vector<int> parameter_sizes(2); - parameter_sizes[0] = 3; - parameter_sizes[1] = 2; - std::vector<int> local_parameter_sizes(2); - local_parameter_sizes[0] = 2; - local_parameter_sizes[1] = 2; - - // Test cost function for correctness. - Eigen::Matrix<double, 3, 3, Eigen::RowMajor> j1_out; - Eigen::Matrix<double, 3, 2, Eigen::RowMajor> j2_out; - Eigen::Vector3d residual; - std::vector<const double*> parameters(2); - parameters[0] = param0.data(); - parameters[1] = param1.data(); - std::vector<double*> jacobians(2); - jacobians[0] = j1_out.data(); - jacobians[1] = j2_out.data(); - cost_function.Evaluate(parameters.data(), residual.data(), jacobians.data()); - - Matrix residual_expected = residual_offset + j0 * param0 + j1 * param1; - - ExpectMatricesClose(j1_out, j0, std::numeric_limits<double>::epsilon()); - ExpectMatricesClose(j2_out, j1, std::numeric_limits<double>::epsilon()); - ExpectMatricesClose(residual, residual_expected, kTolerance); - - // Create parameterization. - Eigen::Matrix<double, 3, 2, Eigen::RowMajor> global_to_local; - global_to_local.row(0) << 1.5, 2.5; - global_to_local.row(1) << 3.5, 4.5; - global_to_local.row(2) << 5.5, 6.5; - - MatrixParameterization parameterization; - parameterization.global_to_local_ = global_to_local; - - // Test local parameterization for correctness. - Eigen::Vector3d x(7.0, 8.0, 9.0); - Eigen::Vector2d delta(10.0, 11.0); - - Eigen::Matrix<double, 3, 2, Eigen::RowMajor> global_to_local_out; - parameterization.ComputeJacobian(x.data(), global_to_local_out.data()); - ExpectMatricesClose(global_to_local_out, - global_to_local, - std::numeric_limits<double>::epsilon()); - - Eigen::Vector3d x_plus_delta; - parameterization.Plus(x.data(), delta.data(), x_plus_delta.data()); - Eigen::Vector3d x_plus_delta_expected = x + (global_to_local * delta); - ExpectMatricesClose(x_plus_delta, x_plus_delta_expected, kTolerance); - - // Now test GradientChecker. - std::vector<const LocalParameterization*> parameterizations(2); - parameterizations[0] = ¶meterization; - parameterizations[1] = nullptr; - NumericDiffOptions numeric_diff_options; - GradientChecker::ProbeResults results; - GradientChecker gradient_checker( - &cost_function, ¶meterizations, numeric_diff_options); - - Problem::Options problem_options; - problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = DO_NOT_TAKE_OWNERSHIP; - Problem problem(problem_options); - Eigen::Vector3d param0_solver; - Eigen::Vector2d param1_solver; - problem.AddParameterBlock(param0_solver.data(), 3, ¶meterization); - problem.AddParameterBlock(param1_solver.data(), 2); - problem.AddResidualBlock( - &cost_function, nullptr, param0_solver.data(), param1_solver.data()); - - // First test case: everything is correct. - EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, nullptr)); - EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, &results)) - << results.error_log; - - // Check that results contain correct data. - ASSERT_EQ(results.return_value, true); - ExpectMatricesClose( - results.residuals, residual, std::numeric_limits<double>::epsilon()); - CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3); - ExpectMatricesClose( - results.local_jacobians.at(0), j0 * global_to_local, kTolerance); - ExpectMatricesClose(results.local_jacobians.at(1), - j1, - std::numeric_limits<double>::epsilon()); - ExpectMatricesClose( - results.local_numeric_jacobians.at(0), j0 * global_to_local, kTolerance); - ExpectMatricesClose(results.local_numeric_jacobians.at(1), j1, kTolerance); - ExpectMatricesClose( - results.jacobians.at(0), j0, std::numeric_limits<double>::epsilon()); - ExpectMatricesClose( - results.jacobians.at(1), j1, std::numeric_limits<double>::epsilon()); - ExpectMatricesClose(results.numeric_jacobians.at(0), j0, kTolerance); - ExpectMatricesClose(results.numeric_jacobians.at(1), j1, kTolerance); - EXPECT_GE(results.maximum_relative_error, 0.0); - EXPECT_TRUE(results.error_log.empty()); - - // Test interaction with the 'check_gradients' option in Solver. - Solver::Options solver_options; - solver_options.linear_solver_type = DENSE_QR; - solver_options.check_gradients = true; - solver_options.initial_trust_region_radius = 1e10; - Solver solver; - Solver::Summary summary; - - param0_solver = param0; - param1_solver = param1; - solver.Solve(solver_options, &problem, &summary); - EXPECT_EQ(CONVERGENCE, summary.termination_type); - EXPECT_LE(summary.final_cost, 1e-12); - - // Second test case: Mess up reported derivatives with respect to 3rd - // component of 1st parameter. Check should fail. - Eigen::Matrix<double, 3, 3, Eigen::RowMajor> j0_offset; - j0_offset.setZero(); - j0_offset.col(2).setConstant(0.001); - cost_function.SetJacobianOffset(0, j0_offset); - EXPECT_FALSE(gradient_checker.Probe(parameters.data(), kTolerance, nullptr)); - EXPECT_FALSE(gradient_checker.Probe(parameters.data(), kTolerance, &results)) - << results.error_log; - - // Check that results contain correct data. - ASSERT_EQ(results.return_value, true); - ExpectMatricesClose( - results.residuals, residual, std::numeric_limits<double>::epsilon()); - CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3); - ASSERT_EQ(results.local_jacobians.size(), 2); - ASSERT_EQ(results.local_numeric_jacobians.size(), 2); - ExpectMatricesClose(results.local_jacobians.at(0), - (j0 + j0_offset) * global_to_local, - kTolerance); - ExpectMatricesClose(results.local_jacobians.at(1), - j1, - std::numeric_limits<double>::epsilon()); - ExpectMatricesClose( - results.local_numeric_jacobians.at(0), j0 * global_to_local, kTolerance); - ExpectMatricesClose(results.local_numeric_jacobians.at(1), j1, kTolerance); - ExpectMatricesClose(results.jacobians.at(0), j0 + j0_offset, kTolerance); - ExpectMatricesClose( - results.jacobians.at(1), j1, std::numeric_limits<double>::epsilon()); - ExpectMatricesClose(results.numeric_jacobians.at(0), j0, kTolerance); - ExpectMatricesClose(results.numeric_jacobians.at(1), j1, kTolerance); - EXPECT_GT(results.maximum_relative_error, 0.0); - EXPECT_FALSE(results.error_log.empty()); - - // Test interaction with the 'check_gradients' option in Solver. - param0_solver = param0; - param1_solver = param1; - solver.Solve(solver_options, &problem, &summary); - EXPECT_EQ(FAILURE, summary.termination_type); - - // Now, zero out the local parameterization Jacobian with respect to the 3rd - // component of the 1st parameter. This makes the combination of cost function - // and local parameterization return correct values again. - parameterization.global_to_local_.row(2).setZero(); - - // Verify that the gradient checker does not treat this as an error. - EXPECT_TRUE(gradient_checker.Probe(parameters.data(), kTolerance, &results)) - << results.error_log; - - // Check that results contain correct data. - ASSERT_EQ(results.return_value, true); - ExpectMatricesClose( - results.residuals, residual, std::numeric_limits<double>::epsilon()); - CheckDimensions(results, parameter_sizes, local_parameter_sizes, 3); - ASSERT_EQ(results.local_jacobians.size(), 2); - ASSERT_EQ(results.local_numeric_jacobians.size(), 2); - ExpectMatricesClose(results.local_jacobians.at(0), - (j0 + j0_offset) * parameterization.global_to_local_, - kTolerance); - ExpectMatricesClose(results.local_jacobians.at(1), - j1, - std::numeric_limits<double>::epsilon()); - ExpectMatricesClose(results.local_numeric_jacobians.at(0), - j0 * parameterization.global_to_local_, - kTolerance); - ExpectMatricesClose(results.local_numeric_jacobians.at(1), j1, kTolerance); - ExpectMatricesClose(results.jacobians.at(0), j0 + j0_offset, kTolerance); - ExpectMatricesClose( - results.jacobians.at(1), j1, std::numeric_limits<double>::epsilon()); - ExpectMatricesClose(results.numeric_jacobians.at(0), j0, kTolerance); - ExpectMatricesClose(results.numeric_jacobians.at(1), j1, kTolerance); - EXPECT_GE(results.maximum_relative_error, 0.0); - EXPECT_TRUE(results.error_log.empty()); - - // Test interaction with the 'check_gradients' option in Solver. - param0_solver = param0; - param1_solver = param1; - solver.Solve(solver_options, &problem, &summary); - EXPECT_EQ(CONVERGENCE, summary.termination_type); - EXPECT_LE(summary.final_cost, 1e-12); -} - // Helper manifold that multiplies the delta vector by the given // jacobian and adds it to the parameter. class MatrixManifold : public Manifold {
diff --git a/internal/ceres/gradient_checking_cost_function_test.cc b/internal/ceres/gradient_checking_cost_function_test.cc index ba5c013..5c6046f 100644 --- a/internal/ceres/gradient_checking_cost_function_test.cc +++ b/internal/ceres/gradient_checking_cost_function_test.cc
@@ -36,7 +36,6 @@ #include <vector> #include "ceres/cost_function.h" -#include "ceres/local_parameterization.h" #include "ceres/loss_function.h" #include "ceres/manifold.h" #include "ceres/parameter_block.h" @@ -336,85 +335,6 @@ EXPECT_EQ(left->IsConstant(), right->IsConstant()); } -TEST(GradientCheckingProblemImpl, - ProblemDimensionsMatchUsingLocalParameterization) { - // Parameter blocks with arbitrarily chosen initial values. - double x[] = {1.0, 2.0, 3.0}; - double y[] = {4.0, 5.0, 6.0, 7.0}; - double z[] = {8.0, 9.0, 10.0, 11.0, 12.0}; - double w[] = {13.0, 14.0, 15.0, 16.0}; - - ProblemImpl problem_impl; - problem_impl.AddParameterBlock(x, 3); - problem_impl.AddParameterBlock(y, 4); - problem_impl.SetParameterBlockConstant(y); - problem_impl.AddParameterBlock(z, 5); - problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization); - // clang-format off - problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), - nullptr, x); - problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4), - nullptr, z, y); - problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5), - new TrivialLoss, x, z); - problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3), - nullptr, z, x); - problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), - nullptr, z, x, y); - // clang-format on - - GradientCheckingIterationCallback callback; - auto gradient_checking_problem_impl = - CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0, &callback); - - // The dimensions of the two problems match. - EXPECT_EQ(problem_impl.NumParameterBlocks(), - gradient_checking_problem_impl->NumParameterBlocks()); - EXPECT_EQ(problem_impl.NumResidualBlocks(), - gradient_checking_problem_impl->NumResidualBlocks()); - - EXPECT_EQ(problem_impl.NumParameters(), - gradient_checking_problem_impl->NumParameters()); - EXPECT_EQ(problem_impl.NumResiduals(), - gradient_checking_problem_impl->NumResiduals()); - - const Program& program = problem_impl.program(); - const Program& gradient_checking_program = - gradient_checking_problem_impl->program(); - - // Since we added the ParameterBlocks and ResidualBlocks explicitly, - // they should be in the same order in the two programs. It is - // possible that may change due to implementation changes to - // Program. This is not expected to be the case and writing code to - // anticipate that possibility not worth the extra complexity in - // this test. - for (int i = 0; i < program.parameter_blocks().size(); ++i) { - ParameterBlocksAreEquivalent( - program.parameter_blocks()[i], - gradient_checking_program.parameter_blocks()[i]); - } - - for (int i = 0; i < program.residual_blocks().size(); ++i) { - // Compare the sizes of the two ResidualBlocks. - const ResidualBlock* original_residual_block = program.residual_blocks()[i]; - const ResidualBlock* new_residual_block = - gradient_checking_program.residual_blocks()[i]; - EXPECT_EQ(original_residual_block->NumParameterBlocks(), - new_residual_block->NumParameterBlocks()); - EXPECT_EQ(original_residual_block->NumResiduals(), - new_residual_block->NumResiduals()); - EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(), - new_residual_block->NumScratchDoublesForEvaluate()); - - // Verify that the ParameterBlocks for the two residuals are equivalent. - for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) { - ParameterBlocksAreEquivalent( - original_residual_block->parameter_blocks()[j], - new_residual_block->parameter_blocks()[j]); - } - } -} - TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) { // Parameter blocks with arbitrarily chosen initial values. double x[] = {1.0, 2.0, 3.0};
diff --git a/internal/ceres/gradient_problem.cc b/internal/ceres/gradient_problem.cc index cdd472f..9d54083 100644 --- a/internal/ceres/gradient_problem.cc +++ b/internal/ceres/gradient_problem.cc
@@ -32,8 +32,6 @@ #include <memory> -#include "ceres/local_parameterization.h" -#include "ceres/manifold_adapter.h" #include "glog/logging.h" namespace ceres { @@ -47,22 +45,6 @@ } GradientProblem::GradientProblem(FirstOrderFunction* function, - LocalParameterization* parameterization) - : function_(function), - parameterization_(parameterization), - scratch_(new double[function_->NumParameters()]) { - CHECK(function != nullptr); - if (parameterization != nullptr) { - manifold_ = - std::make_unique<internal::ManifoldAdapter>(parameterization_.get()); - } else { - manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>( - function_->NumParameters()); - } - CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize()); -} - -GradientProblem::GradientProblem(FirstOrderFunction* function, Manifold* manifold) : function_(function), scratch_(new double[function_->NumParameters()]) { CHECK(function != nullptr);
diff --git a/internal/ceres/gradient_problem_evaluator.h b/internal/ceres/gradient_problem_evaluator.h index 8f74289..f1a61b1 100644 --- a/internal/ceres/gradient_problem_evaluator.h +++ b/internal/ceres/gradient_problem_evaluator.h
@@ -82,7 +82,7 @@ int NumParameters() const final { return problem_.NumParameters(); } int NumEffectiveParameters() const final { - return problem_.NumLocalParameters(); + return problem_.NumTangentParameters(); } int NumResiduals() const final { return 1; }
diff --git a/internal/ceres/gradient_problem_solver.cc b/internal/ceres/gradient_problem_solver.cc index ad3c80e..114b0ee 100644 --- a/internal/ceres/gradient_problem_solver.cc +++ b/internal/ceres/gradient_problem_solver.cc
@@ -113,7 +113,7 @@ *summary = Summary(); // clang-format off summary->num_parameters = problem.NumParameters(); - summary->num_local_parameters = problem.NumLocalParameters(); + summary->num_tangent_parameters = problem.NumTangentParameters(); summary->line_search_direction_type = options.line_search_direction_type; // NOLINT summary->line_search_interpolation_type = options.line_search_interpolation_type; // NOLINT summary->line_search_type = options.line_search_type; @@ -225,8 +225,9 @@ string{"\nSolver Summary (v "}.append(VersionString()) + ")\n\n"; StringAppendF(&report, "Parameters % 25d\n", num_parameters); - if (num_local_parameters != num_parameters) { - StringAppendF(&report, "Local parameters % 25d\n", num_local_parameters); + if (num_tangent_parameters != num_parameters) { + StringAppendF( + &report, "Tangent parameters % 25d\n", num_tangent_parameters); } string line_search_direction_string;
diff --git a/internal/ceres/gradient_problem_test.cc b/internal/ceres/gradient_problem_test.cc index 123ec75..e862cc9 100644 --- a/internal/ceres/gradient_problem_test.cc +++ b/internal/ceres/gradient_problem_test.cc
@@ -68,42 +68,6 @@ EXPECT_TRUE(is_destructed); } -TEST(GradientProblem, EvaluationWithoutParameterizationOrGradient) { - ceres::GradientProblem problem(new QuadraticTestFunction()); - double x = 7.0; - double cost = 0; - problem.Evaluate(&x, &cost, nullptr); - EXPECT_EQ(x * x, cost); -} - -TEST(GradientProblem, EvaluationWithParameterizationAndNoGradient) { - ceres::GradientProblem problem(new QuadraticTestFunction(), - new IdentityParameterization(1)); - double x = 7.0; - double cost = 0; - problem.Evaluate(&x, &cost, nullptr); - EXPECT_EQ(x * x, cost); -} - -TEST(GradientProblem, EvaluationWithoutParameterizationAndWithGradient) { - ceres::GradientProblem problem(new QuadraticTestFunction()); - double x = 7.0; - double cost = 0; - double gradient = 0; - problem.Evaluate(&x, &cost, &gradient); - EXPECT_EQ(2.0 * x, gradient); -} - -TEST(GradientProblem, EvaluationWithParameterizationAndWithGradient) { - ceres::GradientProblem problem(new QuadraticTestFunction(), - new IdentityParameterization(1)); - double x = 7.0; - double cost = 0; - double gradient = 0; - problem.Evaluate(&x, &cost, &gradient); - EXPECT_EQ(2.0 * x, gradient); -} - TEST(GradientProblem, EvaluationWithManifoldAndNoGradient) { ceres::GradientProblem problem(new QuadraticTestFunction(), new EuclideanManifold<1>);
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc deleted file mode 100644 index db6f95a..0000000 --- a/internal/ceres/local_parameterization.cc +++ /dev/null
@@ -1,349 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2022 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) - -#include "ceres/local_parameterization.h" - -#include <algorithm> - -#include "Eigen/Geometry" -#include "ceres/internal/eigen.h" -#include "ceres/internal/fixed_array.h" -#include "ceres/internal/householder_vector.h" -#include "ceres/rotation.h" -#include "glog/logging.h" - -namespace ceres { - -using std::vector; - -LocalParameterization::~LocalParameterization() = default; - -bool LocalParameterization::MultiplyByJacobian(const double* x, - const int num_rows, - const double* global_matrix, - double* local_matrix) const { - if (LocalSize() == 0) { - return true; - } - - Matrix jacobian(GlobalSize(), LocalSize()); - if (!ComputeJacobian(x, jacobian.data())) { - return false; - } - - MatrixRef(local_matrix, num_rows, LocalSize()) = - ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian; - return true; -} - -IdentityParameterization::IdentityParameterization(const int size) - : size_(size) { - CHECK_GT(size, 0); -} - -bool IdentityParameterization::Plus(const double* x, - const double* delta, - double* x_plus_delta) const { - VectorRef(x_plus_delta, size_) = - ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); - return true; -} - -bool IdentityParameterization::ComputeJacobian(const double* x, - double* jacobian) const { - MatrixRef(jacobian, size_, size_).setIdentity(); - return true; -} - -bool IdentityParameterization::MultiplyByJacobian(const double* x, - const int num_cols, - const double* global_matrix, - double* local_matrix) const { - std::copy( - global_matrix, global_matrix + num_cols * GlobalSize(), local_matrix); - return true; -} - -SubsetParameterization::SubsetParameterization( - int size, const vector<int>& constant_parameters) - : local_size_(size - constant_parameters.size()), constancy_mask_(size, 0) { - if (constant_parameters.empty()) { - return; - } - - vector<int> constant = constant_parameters; - std::sort(constant.begin(), constant.end()); - CHECK_GE(constant.front(), 0) << "Indices indicating constant parameter must " - "be greater than equal to zero."; - CHECK_LT(constant.back(), size) - << "Indices indicating constant parameter must be less than the size " - << "of the parameter block."; - CHECK(std::adjacent_find(constant.begin(), constant.end()) == constant.end()) - << "The set of constant parameters cannot contain duplicates"; - for (int parameter : constant_parameters) { - constancy_mask_[parameter] = 1; - } -} - -bool SubsetParameterization::Plus(const double* x, - const double* delta, - double* x_plus_delta) const { - const int global_size = GlobalSize(); - for (int i = 0, j = 0; i < global_size; ++i) { - if (constancy_mask_[i]) { - x_plus_delta[i] = x[i]; - } else { - x_plus_delta[i] = x[i] + delta[j++]; - } - } - return true; -} - -bool SubsetParameterization::ComputeJacobian(const double* x, - double* jacobian) const { - if (local_size_ == 0) { - return true; - } - - const int global_size = GlobalSize(); - MatrixRef m(jacobian, global_size, local_size_); - m.setZero(); - for (int i = 0, j = 0; i < global_size; ++i) { - if (!constancy_mask_[i]) { - m(i, j++) = 1.0; - } - } - return true; -} - -bool SubsetParameterization::MultiplyByJacobian(const double* x, - const int num_cols, - const double* global_matrix, - double* local_matrix) const { - if (local_size_ == 0) { - return true; - } - - const int global_size = GlobalSize(); - for (int col = 0; col < num_cols; ++col) { - for (int i = 0, j = 0; i < global_size; ++i) { - if (!constancy_mask_[i]) { - local_matrix[col * local_size_ + j++] = - global_matrix[col * global_size + i]; - } - } - } - return true; -} - -bool QuaternionParameterization::Plus(const double* x, - const double* delta, - double* x_plus_delta) const { - const double norm_delta = - sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - if (norm_delta > 0.0) { - const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); - double q_delta[4]; - q_delta[0] = cos(norm_delta); - q_delta[1] = sin_delta_by_delta * delta[0]; - q_delta[2] = sin_delta_by_delta * delta[1]; - q_delta[3] = sin_delta_by_delta * delta[2]; - QuaternionProduct(q_delta, x, x_plus_delta); - } else { - for (int i = 0; i < 4; ++i) { - x_plus_delta[i] = x[i]; - } - } - return true; -} - -bool QuaternionParameterization::ComputeJacobian(const double* x, - double* jacobian) const { - // clang-format off - jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; - jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; - jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; - jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; - // clang-format on - return true; -} - -bool EigenQuaternionParameterization::Plus(const double* x_ptr, - const double* delta, - double* x_plus_delta_ptr) const { - Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr); - Eigen::Map<const Eigen::Quaterniond> x(x_ptr); - - const double norm_delta = - sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - if (norm_delta > 0.0) { - const double sin_delta_by_delta = sin(norm_delta) / norm_delta; - - // Note, in the constructor w is first. - Eigen::Quaterniond delta_q(cos(norm_delta), - sin_delta_by_delta * delta[0], - sin_delta_by_delta * delta[1], - sin_delta_by_delta * delta[2]); - x_plus_delta = delta_q * x; - } else { - x_plus_delta = x; - } - - return true; -} - -bool EigenQuaternionParameterization::ComputeJacobian(const double* x, - double* jacobian) const { - // clang-format off - jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; - jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; - jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; - jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; - // clang-format on - return true; -} - -HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size) - : size_(size) { - CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be " - << "greater than 1."; -} - -bool HomogeneousVectorParameterization::Plus(const double* x_ptr, - const double* delta_ptr, - double* x_plus_delta_ptr) const { - ConstVectorRef x(x_ptr, size_); - ConstVectorRef delta(delta_ptr, size_ - 1); - VectorRef x_plus_delta(x_plus_delta_ptr, size_); - - const double norm_delta = delta.norm(); - - if (norm_delta == 0.0) { - x_plus_delta = x; - return true; - } - - // 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; - const double sin_delta_by_delta = - std::sin(norm_delta_div_2) / norm_delta_div_2; - - Vector y(size_); - y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta; - y(size_ - 1) = std::cos(norm_delta_div_2); - - Vector v(size_); - 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<ConstVectorRef, double, Eigen::Dynamic>( - x, &v, &beta); - - // 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. - x_plus_delta = x.norm() * (y - v * (beta * (v.transpose() * y))); - - return true; -} - -bool HomogeneousVectorParameterization::ComputeJacobian( - const double* x_ptr, double* jacobian_ptr) const { - ConstVectorRef x(x_ptr, size_); - MatrixRef jacobian(jacobian_ptr, size_, size_ - 1); - - Vector v(size_); - 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<ConstVectorRef, double, Eigen::Dynamic>( - x, &v, &beta); - - // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the - // Householder matrix (H = I - beta * v * v'). - for (int i = 0; i < size_ - 1; ++i) { - jacobian.col(i) = -0.5 * beta * v(i) * v; - jacobian.col(i)(i) += 0.5; - } - jacobian *= x.norm(); - - return true; -} - -bool ProductParameterization::Plus(const double* x, - const double* delta, - double* x_plus_delta) const { - int x_cursor = 0; - int delta_cursor = 0; - for (const auto& param : local_params_) { - if (!param->Plus( - x + x_cursor, delta + delta_cursor, x_plus_delta + x_cursor)) { - return false; - } - delta_cursor += param->LocalSize(); - x_cursor += param->GlobalSize(); - } - - return true; -} - -bool ProductParameterization::ComputeJacobian(const double* x, - double* jacobian_ptr) const { - MatrixRef jacobian(jacobian_ptr, GlobalSize(), LocalSize()); - jacobian.setZero(); - internal::FixedArray<double> buffer(buffer_size_); - - int x_cursor = 0; - int delta_cursor = 0; - for (const auto& param : local_params_) { - const int local_size = param->LocalSize(); - const int global_size = param->GlobalSize(); - - if (!param->ComputeJacobian(x + x_cursor, buffer.data())) { - return false; - } - jacobian.block(x_cursor, delta_cursor, global_size, local_size) = - MatrixRef(buffer.data(), global_size, local_size); - - delta_cursor += local_size; - x_cursor += global_size; - } - - return true; -} - -} // namespace ceres
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc deleted file mode 100644 index cf54758..0000000 --- a/internal/ceres/local_parameterization_test.cc +++ /dev/null
@@ -1,953 +0,0 @@ -// Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2022 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) - -#include "ceres/local_parameterization.h" - -#include <cmath> -#include <limits> -#include <memory> - -#include "Eigen/Geometry" -#include "ceres/autodiff_local_parameterization.h" -#include "ceres/internal/autodiff.h" -#include "ceres/internal/eigen.h" -#include "ceres/internal/householder_vector.h" -#include "ceres/random.h" -#include "ceres/rotation.h" -#include "gtest/gtest.h" - -namespace ceres { -namespace internal { - -TEST(IdentityParameterization, EverythingTest) { - IdentityParameterization parameterization(3); - EXPECT_EQ(parameterization.GlobalSize(), 3); - EXPECT_EQ(parameterization.LocalSize(), 3); - - double x[3] = {1.0, 2.0, 3.0}; - double delta[3] = {0.0, 1.0, 2.0}; - double x_plus_delta[3] = {0.0, 0.0, 0.0}; - parameterization.Plus(x, delta, x_plus_delta); - EXPECT_EQ(x_plus_delta[0], 1.0); - EXPECT_EQ(x_plus_delta[1], 3.0); - EXPECT_EQ(x_plus_delta[2], 5.0); - - double jacobian[9]; - parameterization.ComputeJacobian(x, jacobian); - int k = 0; - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j, ++k) { - EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0); - } - } - - Matrix global_matrix = Matrix::Ones(10, 3); - Matrix local_matrix = Matrix::Zero(10, 3); - parameterization.MultiplyByJacobian( - x, 10, global_matrix.data(), local_matrix.data()); - EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0); -} - -TEST(SubsetParameterization, EmptyConstantParameters) { - std::vector<int> constant_parameters; - SubsetParameterization parameterization(3, constant_parameters); - EXPECT_EQ(parameterization.GlobalSize(), 3); - EXPECT_EQ(parameterization.LocalSize(), 3); - double x[3] = {1, 2, 3}; - double delta[3] = {4, 5, 6}; - double x_plus_delta[3] = {-1, -2, -3}; - parameterization.Plus(x, delta, x_plus_delta); - EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]); - EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]); - EXPECT_EQ(x_plus_delta[2], x[2] + delta[2]); - - Matrix jacobian(3, 3); - Matrix expected_jacobian(3, 3); - expected_jacobian.setIdentity(); - parameterization.ComputeJacobian(x, jacobian.data()); - EXPECT_EQ(jacobian, expected_jacobian); - - Matrix global_matrix(3, 5); - global_matrix.setRandom(); - Matrix local_matrix(3, 5); - parameterization.MultiplyByJacobian( - x, 5, global_matrix.data(), local_matrix.data()); - EXPECT_EQ(global_matrix, local_matrix); -} - -TEST(SubsetParameterization, NegativeParameterIndexDeathTest) { - std::vector<int> constant_parameters; - constant_parameters.push_back(-1); - EXPECT_DEATH_IF_SUPPORTED( - SubsetParameterization parameterization(2, constant_parameters), - "greater than equal to zero"); -} - -TEST(SubsetParameterization, GreaterThanSizeParameterIndexDeathTest) { - std::vector<int> constant_parameters; - constant_parameters.push_back(2); - EXPECT_DEATH_IF_SUPPORTED( - SubsetParameterization parameterization(2, constant_parameters), - "less than the size"); -} - -TEST(SubsetParameterization, DuplicateParametersDeathTest) { - std::vector<int> constant_parameters; - constant_parameters.push_back(1); - constant_parameters.push_back(1); - EXPECT_DEATH_IF_SUPPORTED( - SubsetParameterization parameterization(2, constant_parameters), - "duplicates"); -} - -TEST(SubsetParameterization, - ProductParameterizationWithZeroLocalSizeSubsetParameterization1) { - std::vector<int> constant_parameters; - constant_parameters.push_back(0); - LocalParameterization* subset_param = - new SubsetParameterization(1, constant_parameters); - LocalParameterization* identity_param = new IdentityParameterization(2); - ProductParameterization product_param(subset_param, identity_param); - EXPECT_EQ(product_param.GlobalSize(), 3); - EXPECT_EQ(product_param.LocalSize(), 2); - double x[] = {1.0, 1.0, 1.0}; - double delta[] = {2.0, 3.0}; - double x_plus_delta[] = {0.0, 0.0, 0.0}; - EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta)); - EXPECT_EQ(x_plus_delta[0], x[0]); - EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]); - EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]); - - Matrix actual_jacobian(3, 2); - EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data())); -} - -TEST(SubsetParameterization, - ProductParameterizationWithZeroLocalSizeSubsetParameterization2) { - std::vector<int> constant_parameters; - constant_parameters.push_back(0); - LocalParameterization* subset_param = - new SubsetParameterization(1, constant_parameters); - LocalParameterization* identity_param = new IdentityParameterization(2); - ProductParameterization product_param(identity_param, subset_param); - EXPECT_EQ(product_param.GlobalSize(), 3); - EXPECT_EQ(product_param.LocalSize(), 2); - double x[] = {1.0, 1.0, 1.0}; - double delta[] = {2.0, 3.0}; - double x_plus_delta[] = {0.0, 0.0, 0.0}; - EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta)); - EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]); - EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]); - EXPECT_EQ(x_plus_delta[2], x[2]); - - Matrix actual_jacobian(3, 2); - EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data())); -} - -TEST(SubsetParameterization, NormalFunctionTest) { - const int kGlobalSize = 4; - const int kLocalSize = 3; - - double x[kGlobalSize] = {1.0, 2.0, 3.0, 4.0}; - for (int i = 0; i < kGlobalSize; ++i) { - std::vector<int> constant_parameters; - constant_parameters.push_back(i); - SubsetParameterization parameterization(kGlobalSize, constant_parameters); - double delta[kLocalSize] = {1.0, 2.0, 3.0}; - double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0}; - - parameterization.Plus(x, delta, x_plus_delta); - int k = 0; - for (int j = 0; j < kGlobalSize; ++j) { - if (j == i) { - EXPECT_EQ(x_plus_delta[j], x[j]); - } else { - EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]); - } - } - - double jacobian[kGlobalSize * kLocalSize]; - parameterization.ComputeJacobian(x, jacobian); - int delta_cursor = 0; - int jacobian_cursor = 0; - for (int j = 0; j < kGlobalSize; ++j) { - if (j != i) { - for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) { - EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0); - } - ++delta_cursor; - } else { - for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) { - EXPECT_EQ(jacobian[jacobian_cursor], 0.0); - } - } - } - - Matrix global_matrix = Matrix::Ones(10, kGlobalSize); - for (int row = 0; row < kGlobalSize; ++row) { - for (int col = 0; col < kGlobalSize; ++col) { - global_matrix(row, col) = col; - } - } - - Matrix local_matrix = Matrix::Zero(10, kLocalSize); - parameterization.MultiplyByJacobian( - x, 10, global_matrix.data(), local_matrix.data()); - Matrix expected_local_matrix = - global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize); - EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0); - } -} - -// Functor needed to implement automatically differentiated Plus for -// quaternions. -struct QuaternionPlus { - template <typename T> - bool operator()(const T* x, const T* delta, T* x_plus_delta) const { - const T squared_norm_delta = - delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; - - T q_delta[4]; - if (squared_norm_delta > T(0.0)) { - T norm_delta = sqrt(squared_norm_delta); - const T sin_delta_by_delta = sin(norm_delta) / norm_delta; - q_delta[0] = cos(norm_delta); - q_delta[1] = sin_delta_by_delta * delta[0]; - q_delta[2] = sin_delta_by_delta * delta[1]; - q_delta[3] = sin_delta_by_delta * delta[2]; - } else { - // We do not just use q_delta = [1,0,0,0] here because that is a - // constant and when used for automatic differentiation will - // lead to a zero derivative. Instead we take a first order - // approximation and evaluate it at zero. - q_delta[0] = T(1.0); - q_delta[1] = delta[0]; - q_delta[2] = delta[1]; - q_delta[3] = delta[2]; - } - - QuaternionProduct(q_delta, x, x_plus_delta); - return true; - } -}; - -template <typename Parameterization, typename Plus> -void QuaternionParameterizationTestHelper(const double* x, - const double* delta, - const double* x_plus_delta_ref) { - const int kGlobalSize = 4; - const int kLocalSize = 3; - - const double kTolerance = 1e-14; - - double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0, 0.0}; - Parameterization parameterization; - parameterization.Plus(x, delta, x_plus_delta); - for (int i = 0; i < kGlobalSize; ++i) { - EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); - } - - const double x_plus_delta_norm = sqrt( - x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] + - x_plus_delta[2] * x_plus_delta[2] + x_plus_delta[3] * x_plus_delta[3]); - - EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance); - - double jacobian_ref[12]; - double zero_delta[kLocalSize] = {0.0, 0.0, 0.0}; - const double* parameters[2] = {x, zero_delta}; - double* jacobian_array[2] = {nullptr, jacobian_ref}; - - // Autodiff jacobian at delta_x = 0. - internal::AutoDifferentiate<kGlobalSize, - StaticParameterDims<kGlobalSize, kLocalSize>>( - Plus(), parameters, kGlobalSize, x_plus_delta, jacobian_array); - - double jacobian[12]; - parameterization.ComputeJacobian(x, jacobian); - for (int i = 0; i < 12; ++i) { - EXPECT_TRUE(isfinite(jacobian[i])); - EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) - << "Jacobian mismatch: i = " << i << "\n Expected \n" - << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize) - << "\n Actual \n" - << ConstMatrixRef(jacobian, kGlobalSize, kLocalSize); - } - - Matrix global_matrix = Matrix::Random(10, kGlobalSize); - Matrix local_matrix = Matrix::Zero(10, kLocalSize); - parameterization.MultiplyByJacobian( - x, 10, global_matrix.data(), local_matrix.data()); - Matrix expected_local_matrix = - global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize); - EXPECT_NEAR((local_matrix - expected_local_matrix).norm(), - 0.0, - 10.0 * std::numeric_limits<double>::epsilon()); -} - -template <int N> -void Normalize(double* x) { - VectorRef(x, N).normalize(); -} - -TEST(QuaternionParameterization, ZeroTest) { - double x[4] = {0.5, 0.5, 0.5, 0.5}; - double delta[3] = {0.0, 0.0, 0.0}; - double q_delta[4] = {1.0, 0.0, 0.0, 0.0}; - double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; - QuaternionProduct(q_delta, x, x_plus_delta); - QuaternionParameterizationTestHelper<QuaternionParameterization, - QuaternionPlus>(x, delta, x_plus_delta); -} - -TEST(QuaternionParameterization, NearZeroTest) { - double x[4] = {0.52, 0.25, 0.15, 0.45}; - Normalize<4>(x); - - double delta[3] = {0.24, 0.15, 0.10}; - for (double& delta_i : delta) { - delta_i = delta_i * 1e-14; - } - - double q_delta[4]; - q_delta[0] = 1.0; - q_delta[1] = delta[0]; - q_delta[2] = delta[1]; - q_delta[3] = delta[2]; - - double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; - QuaternionProduct(q_delta, x, x_plus_delta); - QuaternionParameterizationTestHelper<QuaternionParameterization, - QuaternionPlus>(x, delta, x_plus_delta); -} - -TEST(QuaternionParameterization, AwayFromZeroTest) { - double x[4] = {0.52, 0.25, 0.15, 0.45}; - Normalize<4>(x); - - double delta[3] = {0.24, 0.15, 0.10}; - const double delta_norm = - sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - double q_delta[4]; - q_delta[0] = cos(delta_norm); - q_delta[1] = sin(delta_norm) / delta_norm * delta[0]; - q_delta[2] = sin(delta_norm) / delta_norm * delta[1]; - q_delta[3] = sin(delta_norm) / delta_norm * delta[2]; - - double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; - QuaternionProduct(q_delta, x, x_plus_delta); - QuaternionParameterizationTestHelper<QuaternionParameterization, - QuaternionPlus>(x, delta, x_plus_delta); -} - -// Functor needed to implement automatically differentiated Plus for -// Eigen's quaternion. -struct EigenQuaternionPlus { - template <typename T> - bool operator()(const T* x, const T* delta, T* x_plus_delta) const { - const T norm_delta = - sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - - Eigen::Quaternion<T> q_delta; - if (norm_delta > T(0.0)) { - const T sin_delta_by_delta = sin(norm_delta) / norm_delta; - q_delta.coeffs() << sin_delta_by_delta * delta[0], - sin_delta_by_delta * delta[1], sin_delta_by_delta * delta[2], - cos(norm_delta); - } else { - // We do not just use q_delta = [0,0,0,1] here because that is a - // constant and when used for automatic differentiation will - // lead to a zero derivative. Instead we take a first order - // approximation and evaluate it at zero. - q_delta.coeffs() << delta[0], delta[1], delta[2], T(1.0); - } - - Eigen::Map<Eigen::Quaternion<T>> x_plus_delta_ref(x_plus_delta); - Eigen::Map<const Eigen::Quaternion<T>> x_ref(x); - x_plus_delta_ref = q_delta * x_ref; - return true; - } -}; - -TEST(EigenQuaternionParameterization, ZeroTest) { - Eigen::Quaterniond x(0.5, 0.5, 0.5, 0.5); - double delta[3] = {0.0, 0.0, 0.0}; - Eigen::Quaterniond q_delta(1.0, 0.0, 0.0, 0.0); - Eigen::Quaterniond x_plus_delta = q_delta * x; - QuaternionParameterizationTestHelper<EigenQuaternionParameterization, - EigenQuaternionPlus>( - x.coeffs().data(), delta, x_plus_delta.coeffs().data()); -} - -TEST(EigenQuaternionParameterization, NearZeroTest) { - Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45); - x.normalize(); - - double delta[3] = {0.24, 0.15, 0.10}; - for (double& delta_i : delta) { - delta_i = delta_i * 1e-14; - } - - // Note: w is first in the constructor. - Eigen::Quaterniond q_delta(1.0, delta[0], delta[1], delta[2]); - - Eigen::Quaterniond x_plus_delta = q_delta * x; - QuaternionParameterizationTestHelper<EigenQuaternionParameterization, - EigenQuaternionPlus>( - x.coeffs().data(), delta, x_plus_delta.coeffs().data()); -} - -TEST(EigenQuaternionParameterization, AwayFromZeroTest) { - Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45); - x.normalize(); - - double delta[3] = {0.24, 0.15, 0.10}; - const double delta_norm = - sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - - // Note: w is first in the constructor. - Eigen::Quaterniond q_delta(cos(delta_norm), - sin(delta_norm) / delta_norm * delta[0], - sin(delta_norm) / delta_norm * delta[1], - sin(delta_norm) / delta_norm * delta[2]); - - Eigen::Quaterniond x_plus_delta = q_delta * x; - QuaternionParameterizationTestHelper<EigenQuaternionParameterization, - EigenQuaternionPlus>( - x.coeffs().data(), delta, x_plus_delta.coeffs().data()); -} - -// Functor needed to implement automatically differentiated Plus for -// homogeneous vectors. -template <int Dim> -struct HomogeneousVectorParameterizationPlus { - template <typename Scalar> - bool operator()(const Scalar* p_x, - const Scalar* p_delta, - Scalar* p_x_plus_delta) const { - Eigen::Map<const Eigen::Matrix<Scalar, Dim, 1>> x(p_x); - Eigen::Map<const Eigen::Matrix<Scalar, Dim - 1, 1>> delta(p_delta); - Eigen::Map<Eigen::Matrix<Scalar, Dim, 1>> x_plus_delta(p_x_plus_delta); - - const Scalar squared_norm_delta = delta.squaredNorm(); - - Eigen::Matrix<Scalar, Dim, 1> y; - Scalar one_half(0.5); - if (squared_norm_delta > Scalar(0.0)) { - Scalar norm_delta = sqrt(squared_norm_delta); - Scalar norm_delta_div_2 = 0.5 * norm_delta; - const Scalar sin_delta_by_delta = - sin(norm_delta_div_2) / norm_delta_div_2; - y.template head<Dim - 1>() = sin_delta_by_delta * one_half * delta; - y[Dim - 1] = cos(norm_delta_div_2); - - } else { - // We do not just use y = [0,0,0,1] here because that is a - // constant and when used for automatic differentiation will - // lead to a zero derivative. Instead we take a first order - // approximation and evaluate it at zero. - y.template head<Dim - 1>() = delta * one_half; - y[Dim - 1] = Scalar(1.0); - } - - Eigen::Matrix<Scalar, Dim, 1> v; - Scalar 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< - Eigen::Map<const Eigen::Matrix<Scalar, Dim, 1>>, - Scalar, - Dim>(x, &v, &beta); - - x_plus_delta = x.norm() * (y - v * (beta * v.dot(y))); - - return true; - } -}; - -static void HomogeneousVectorParameterizationHelper(const double* x, - const double* delta) { - const double kTolerance = 1e-14; - - HomogeneousVectorParameterization homogeneous_vector_parameterization(4); - - // Ensure the update maintains the norm. - double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; - homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta); - - const double x_plus_delta_norm = sqrt( - x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] + - x_plus_delta[2] * x_plus_delta[2] + x_plus_delta[3] * x_plus_delta[3]); - - const double x_norm = - sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]); - - EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance); - - // Autodiff jacobian at delta_x = 0. - AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus<4>, 4, 3> - autodiff_jacobian; - - double jacobian_autodiff[12]; - double jacobian_analytic[12]; - - homogeneous_vector_parameterization.ComputeJacobian(x, jacobian_analytic); - autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff); - - for (int i = 0; i < 12; ++i) { - EXPECT_TRUE(ceres::isfinite(jacobian_analytic[i])); - EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance) - << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " " - << jacobian_autodiff[i]; - } -} - -TEST(HomogeneousVectorParameterization, ZeroTest) { - double x[4] = {0.0, 0.0, 0.0, 1.0}; - Normalize<4>(x); - double delta[3] = {0.0, 0.0, 0.0}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, NearZeroTest1) { - double x[4] = {1e-5, 1e-5, 1e-5, 1.0}; - Normalize<4>(x); - double delta[3] = {0.0, 1.0, 0.0}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, NearZeroTest2) { - double x[4] = {0.001, 0.0, 0.0, 0.0}; - double delta[3] = {0.0, 1.0, 0.0}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, AwayFromZeroTest1) { - double x[4] = {0.52, 0.25, 0.15, 0.45}; - Normalize<4>(x); - double delta[3] = {0.0, 1.0, -0.5}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, AwayFromZeroTest2) { - double x[4] = {0.87, -0.25, -0.34, 0.45}; - Normalize<4>(x); - double delta[3] = {0.0, 0.0, -0.5}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, AwayFromZeroTest3) { - double x[4] = {0.0, 0.0, 0.0, 2.0}; - double delta[3] = {0.0, 0.0, 0}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, AwayFromZeroTest4) { - double x[4] = {0.2, -1.0, 0.0, 2.0}; - double delta[3] = {1.4, 0.0, -0.5}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, AwayFromZeroTest5) { - double x[4] = {2.0, 0.0, 0.0, 0.0}; - double delta[3] = {1.4, 0.0, -0.5}; - - HomogeneousVectorParameterizationHelper(x, delta); -} - -TEST(HomogeneousVectorParameterization, DeathTests) { - EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size"); -} - -// Functor needed to implement automatically differentiated Plus for -// line parameterization. -template <int AmbientSpaceDim> -struct LineParameterizationPlus { - template <typename Scalar> - bool operator()(const Scalar* p_x, - const Scalar* p_delta, - Scalar* p_x_plus_delta) const { - static constexpr int kTangentSpaceDim = AmbientSpaceDim - 1; - Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> origin_point( - p_x); - Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> dir( - p_x + AmbientSpaceDim); - Eigen::Map<const Eigen::Matrix<Scalar, kTangentSpaceDim, 1>> - delta_origin_point(p_delta); - Eigen::Map<Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> - origin_point_plus_delta(p_x_plus_delta); - - HomogeneousVectorParameterizationPlus<AmbientSpaceDim> dir_plus; - dir_plus(dir.data(), - p_delta + kTangentSpaceDim, - p_x_plus_delta + AmbientSpaceDim); - - Eigen::Matrix<Scalar, AmbientSpaceDim, 1> v; - Scalar 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< - Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>>, - Scalar, - AmbientSpaceDim>(dir, &v, &beta); - - Eigen::Matrix<Scalar, AmbientSpaceDim, 1> y; - y << 0.5 * delta_origin_point, Scalar(0.0); - origin_point_plus_delta = origin_point + y - v * (beta * v.dot(y)); - - return true; - } -}; - -template <int AmbientSpaceDim> -static void LineParameterizationHelper(const double* x_ptr, - const double* delta) { - const double kTolerance = 1e-14; - - static constexpr int ParameterDim = 2 * AmbientSpaceDim; - static constexpr int TangientParameterDim = 2 * (AmbientSpaceDim - 1); - - LineParameterization<AmbientSpaceDim> line_parameterization; - - using ParameterVector = Eigen::Matrix<double, ParameterDim, 1>; - ParameterVector x_plus_delta = ParameterVector::Zero(); - line_parameterization.Plus(x_ptr, delta, x_plus_delta.data()); - - // Ensure the update maintains the norm for the line direction. - Eigen::Map<const ParameterVector> x(x_ptr); - const double dir_plus_delta_norm = - x_plus_delta.template tail<AmbientSpaceDim>().norm(); - const double dir_norm = x.template tail<AmbientSpaceDim>().norm(); - EXPECT_NEAR(dir_plus_delta_norm, dir_norm, kTolerance); - - // Ensure the update of the origin point is perpendicular to the line - // direction. - const double dot_prod_val = x.template tail<AmbientSpaceDim>().dot( - x_plus_delta.template head<AmbientSpaceDim>() - - x.template head<AmbientSpaceDim>()); - EXPECT_NEAR(dot_prod_val, 0.0, kTolerance); - - // Autodiff jacobian at delta_x = 0. - AutoDiffLocalParameterization<LineParameterizationPlus<AmbientSpaceDim>, - ParameterDim, - TangientParameterDim> - autodiff_jacobian; - - using JacobianMatrix = Eigen:: - Matrix<double, ParameterDim, TangientParameterDim, Eigen::RowMajor>; - constexpr double kNaN = std::numeric_limits<double>::quiet_NaN(); - JacobianMatrix jacobian_autodiff = JacobianMatrix::Constant(kNaN); - JacobianMatrix jacobian_analytic = JacobianMatrix::Constant(kNaN); - - autodiff_jacobian.ComputeJacobian(x_ptr, jacobian_autodiff.data()); - line_parameterization.ComputeJacobian(x_ptr, jacobian_analytic.data()); - - EXPECT_FALSE(jacobian_autodiff.hasNaN()); - EXPECT_FALSE(jacobian_analytic.hasNaN()); - EXPECT_TRUE(jacobian_autodiff.isApprox(jacobian_analytic)) - << "auto diff:\n" - << jacobian_autodiff << "\n" - << "analytic diff:\n" - << jacobian_analytic; -} - -TEST(LineParameterization, ZeroTest3D) { - double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; - double delta[4] = {0.0, 0.0, 0.0, 0.0}; - - LineParameterizationHelper<3>(x, delta); -} - -TEST(LineParameterization, ZeroTest4D) { - double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; - double delta[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - - LineParameterizationHelper<4>(x, delta); -} - -TEST(LineParameterization, ZeroOriginPointTest3D) { - double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; - double delta[4] = {0.0, 0.0, 1.0, 2.0}; - - LineParameterizationHelper<3>(x, delta); -} - -TEST(LineParameterization, ZeroOriginPointTest4D) { - double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; - double delta[6] = {0.0, 0.0, 0.0, 1.0, 2.0, 3.0}; - - LineParameterizationHelper<4>(x, delta); -} - -TEST(LineParameterization, ZeroDirTest3D) { - double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; - double delta[4] = {3.0, 2.0, 0.0, 0.0}; - - LineParameterizationHelper<3>(x, delta); -} - -TEST(LineParameterization, ZeroDirTest4D) { - double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; - double delta[6] = {3.0, 2.0, 1.0, 0.0, 0.0, 0.0}; - - LineParameterizationHelper<4>(x, delta); -} - -TEST(LineParameterization, AwayFromZeroTest3D1) { - Eigen::Matrix<double, 6, 1> x; - x.head<3>() << 1.54, 2.32, 1.34; - x.tail<3>() << 0.52, 0.25, 0.15; - x.tail<3>().normalize(); - - double delta[4] = {4.0, 7.0, 1.0, -0.5}; - - LineParameterizationHelper<3>(x.data(), delta); -} - -TEST(LineParameterization, AwayFromZeroTest4D1) { - Eigen::Matrix<double, 8, 1> x; - x.head<4>() << 1.54, 2.32, 1.34, 3.23; - x.tail<4>() << 0.52, 0.25, 0.15, 0.45; - x.tail<4>().normalize(); - - double delta[6] = {4.0, 7.0, -3.0, 0.0, 1.0, -0.5}; - - LineParameterizationHelper<4>(x.data(), delta); -} - -TEST(LineParameterization, AwayFromZeroTest3D2) { - Eigen::Matrix<double, 6, 1> x; - x.head<3>() << 7.54, -2.81, 8.63; - x.tail<3>() << 2.52, 5.25, 4.15; - - double delta[4] = {4.0, 7.0, 1.0, -0.5}; - - LineParameterizationHelper<3>(x.data(), delta); -} - -TEST(LineParameterization, AwayFromZeroTest4D2) { - Eigen::Matrix<double, 8, 1> x; - x.head<4>() << 7.54, -2.81, 8.63, 6.93; - x.tail<4>() << 2.52, 5.25, 4.15, 1.45; - - double delta[6] = {4.0, 7.0, -3.0, 2.0, 1.0, -0.5}; - - LineParameterizationHelper<4>(x.data(), delta); -} - -class ProductParameterizationTest : public ::testing::Test { - protected: - void SetUp() final { - const int global_size1 = 5; - std::vector<int> constant_parameters1; - constant_parameters1.push_back(2); - param1_ = std::make_unique<SubsetParameterization>(global_size1, - constant_parameters1); - - const int global_size2 = 3; - std::vector<int> constant_parameters2; - constant_parameters2.push_back(0); - constant_parameters2.push_back(1); - param2_ = std::make_unique<SubsetParameterization>(global_size2, - constant_parameters2); - - const int global_size3 = 4; - std::vector<int> constant_parameters3; - constant_parameters3.push_back(1); - param3_ = std::make_unique<SubsetParameterization>(global_size3, - constant_parameters3); - - const int global_size4 = 2; - std::vector<int> constant_parameters4; - constant_parameters4.push_back(1); - param4_ = std::make_unique<SubsetParameterization>(global_size4, - constant_parameters4); - } - - std::unique_ptr<LocalParameterization> param1_; - std::unique_ptr<LocalParameterization> param2_; - std::unique_ptr<LocalParameterization> param3_; - std::unique_ptr<LocalParameterization> param4_; -}; - -TEST_F(ProductParameterizationTest, LocalAndGlobalSize2) { - LocalParameterization* param1 = param1_.release(); - LocalParameterization* param2 = param2_.release(); - - ProductParameterization product_param(param1, param2); - EXPECT_EQ(product_param.LocalSize(), - param1->LocalSize() + param2->LocalSize()); - EXPECT_EQ(product_param.GlobalSize(), - param1->GlobalSize() + param2->GlobalSize()); -} - -TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) { - LocalParameterization* param1 = param1_.release(); - LocalParameterization* param2 = param2_.release(); - LocalParameterization* param3 = param3_.release(); - - ProductParameterization product_param(param1, param2, param3); - EXPECT_EQ(product_param.LocalSize(), - param1->LocalSize() + param2->LocalSize() + param3->LocalSize()); - EXPECT_EQ(product_param.GlobalSize(), - param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize()); -} - -TEST_F(ProductParameterizationTest, LocalAndGlobalSize4) { - LocalParameterization* param1 = param1_.release(); - LocalParameterization* param2 = param2_.release(); - LocalParameterization* param3 = param3_.release(); - LocalParameterization* param4 = param4_.release(); - - ProductParameterization product_param(param1, param2, param3, param4); - EXPECT_EQ(product_param.LocalSize(), - param1->LocalSize() + param2->LocalSize() + param3->LocalSize() + - param4->LocalSize()); - EXPECT_EQ(product_param.GlobalSize(), - param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize() + - param4->GlobalSize()); -} - -TEST_F(ProductParameterizationTest, Plus) { - LocalParameterization* param1 = param1_.release(); - LocalParameterization* param2 = param2_.release(); - LocalParameterization* param3 = param3_.release(); - LocalParameterization* param4 = param4_.release(); - - ProductParameterization product_param(param1, param2, param3, param4); - std::vector<double> x(product_param.GlobalSize(), 0.0); - std::vector<double> delta(product_param.LocalSize(), 0.0); - std::vector<double> x_plus_delta_expected(product_param.GlobalSize(), 0.0); - std::vector<double> x_plus_delta(product_param.GlobalSize(), 0.0); - - for (int i = 0; i < product_param.GlobalSize(); ++i) { - x[i] = RandNormal(); - } - - for (int i = 0; i < product_param.LocalSize(); ++i) { - delta[i] = RandNormal(); - } - - EXPECT_TRUE(product_param.Plus(&x[0], &delta[0], &x_plus_delta_expected[0])); - int x_cursor = 0; - int delta_cursor = 0; - - EXPECT_TRUE(param1->Plus( - &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor])); - x_cursor += param1->GlobalSize(); - delta_cursor += param1->LocalSize(); - - EXPECT_TRUE(param2->Plus( - &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor])); - x_cursor += param2->GlobalSize(); - delta_cursor += param2->LocalSize(); - - EXPECT_TRUE(param3->Plus( - &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor])); - x_cursor += param3->GlobalSize(); - delta_cursor += param3->LocalSize(); - - EXPECT_TRUE(param4->Plus( - &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor])); - x_cursor += param4->GlobalSize(); - delta_cursor += param4->LocalSize(); - - for (int i = 0; i < x.size(); ++i) { - EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]); - } -} - -TEST_F(ProductParameterizationTest, ComputeJacobian) { - LocalParameterization* param1 = param1_.release(); - LocalParameterization* param2 = param2_.release(); - LocalParameterization* param3 = param3_.release(); - LocalParameterization* param4 = param4_.release(); - - ProductParameterization product_param(param1, param2, param3, param4); - std::vector<double> x(product_param.GlobalSize(), 0.0); - - for (int i = 0; i < product_param.GlobalSize(); ++i) { - x[i] = RandNormal(); - } - - Matrix jacobian = - Matrix::Random(product_param.GlobalSize(), product_param.LocalSize()); - EXPECT_TRUE(product_param.ComputeJacobian(&x[0], jacobian.data())); - int x_cursor = 0; - int delta_cursor = 0; - - Matrix jacobian1(param1->GlobalSize(), param1->LocalSize()); - EXPECT_TRUE(param1->ComputeJacobian(&x[x_cursor], jacobian1.data())); - jacobian.block( - x_cursor, delta_cursor, param1->GlobalSize(), param1->LocalSize()) -= - jacobian1; - x_cursor += param1->GlobalSize(); - delta_cursor += param1->LocalSize(); - - Matrix jacobian2(param2->GlobalSize(), param2->LocalSize()); - EXPECT_TRUE(param2->ComputeJacobian(&x[x_cursor], jacobian2.data())); - jacobian.block( - x_cursor, delta_cursor, param2->GlobalSize(), param2->LocalSize()) -= - jacobian2; - x_cursor += param2->GlobalSize(); - delta_cursor += param2->LocalSize(); - - Matrix jacobian3(param3->GlobalSize(), param3->LocalSize()); - EXPECT_TRUE(param3->ComputeJacobian(&x[x_cursor], jacobian3.data())); - jacobian.block( - x_cursor, delta_cursor, param3->GlobalSize(), param3->LocalSize()) -= - jacobian3; - x_cursor += param3->GlobalSize(); - delta_cursor += param3->LocalSize(); - - Matrix jacobian4(param4->GlobalSize(), param4->LocalSize()); - EXPECT_TRUE(param4->ComputeJacobian(&x[x_cursor], jacobian4.data())); - jacobian.block( - x_cursor, delta_cursor, param4->GlobalSize(), param4->LocalSize()) -= - jacobian4; - x_cursor += param4->GlobalSize(); - delta_cursor += param4->LocalSize(); - - EXPECT_NEAR(jacobian.norm(), 0.0, std::numeric_limits<double>::epsilon()); -} - -} // namespace internal -} // namespace ceres
diff --git a/internal/ceres/manifold_adapter.h b/internal/ceres/manifold_adapter.h deleted file mode 100644 index af4c712..0000000 --- a/internal/ceres/manifold_adapter.h +++ /dev/null
@@ -1,58 +0,0 @@ -#include "ceres/internal/export.h" -#include "ceres/local_parameterization.h" -#include "ceres/manifold.h" -#include "glog/logging.h" - -namespace ceres::internal { - -// Adapter to wrap LocalParameterization and make them look like Manifolds. -// -// ManifoldAdapter NEVER takes ownership of local_parameterization. -class CERES_NO_EXPORT ManifoldAdapter final : public Manifold { - public: - explicit ManifoldAdapter(const LocalParameterization* local_parameterization) - : local_parameterization_(local_parameterization) { - CHECK(local_parameterization != nullptr); - } - - bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const override { - return local_parameterization_->Plus(x, delta, x_plus_delta); - } - - bool PlusJacobian(const double* x, double* jacobian) const override { - return local_parameterization_->ComputeJacobian(x, jacobian); - } - - bool RightMultiplyByPlusJacobian(const double* x, - const int num_rows, - const double* ambient_matrix, - double* tangent_matrix) const override { - return local_parameterization_->MultiplyByJacobian( - x, num_rows, ambient_matrix, tangent_matrix); - } - - bool Minus(const double* y, const double* x, double* delta) const override { - LOG(FATAL) << "This should never be called."; - return false; - } - - bool MinusJacobian(const double* x, double* jacobian) const override { - LOG(FATAL) << "This should never be called."; - return false; - } - - int AmbientSize() const override { - return local_parameterization_->GlobalSize(); - } - - int TangentSize() const override { - return local_parameterization_->LocalSize(); - } - - private: - const LocalParameterization* local_parameterization_; -}; - -} // namespace ceres::internal
diff --git a/internal/ceres/problem.cc b/internal/ceres/problem.cc index 4269ca3..f4c2656 100644 --- a/internal/ceres/problem.cc +++ b/internal/ceres/problem.cc
@@ -71,12 +71,6 @@ impl_->AddParameterBlock(values, size); } -void Problem::AddParameterBlock(double* values, - int size, - LocalParameterization* local_parameterization) { - impl_->AddParameterBlock(values, size, local_parameterization); -} - void Problem::AddParameterBlock(double* values, int size, Manifold* manifold) { impl_->AddParameterBlock(values, size, manifold); } @@ -101,20 +95,6 @@ return impl_->IsParameterBlockConstant(values); } -void Problem::SetParameterization( - double* values, LocalParameterization* local_parameterization) { - impl_->SetParameterization(values, local_parameterization); -} - -const LocalParameterization* Problem::GetParameterization( - const double* values) const { - return impl_->GetParameterization(values); -} - -bool Problem::HasParameterization(const double* values) const { - return impl_->HasParameterization(values); -} - void Problem::SetManifold(double* values, Manifold* manifold) { impl_->SetManifold(values, manifold); } @@ -194,10 +174,6 @@ return impl_->ParameterBlockSize(values); } -int Problem::ParameterBlockLocalSize(const double* values) const { - return impl_->ParameterBlockTangentSize(values); -} - int Problem::ParameterBlockTangentSize(const double* values) const { return impl_->ParameterBlockTangentSize(values); }
diff --git a/internal/ceres/problem_impl.cc b/internal/ceres/problem_impl.cc index 8549097..3264166 100644 --- a/internal/ceres/problem_impl.cc +++ b/internal/ceres/problem_impl.cc
@@ -53,7 +53,6 @@ #include "ceres/internal/fixed_array.h" #include "ceres/loss_function.h" #include "ceres/manifold.h" -#include "ceres/manifold_adapter.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" #include "ceres/program.h" @@ -256,10 +255,6 @@ DeleteBlock(parameter_block); } - // Delete the owned parameterizations. - STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), - local_parameterizations_to_delete_.end()); - // Delete the owned manifolds. STLDeleteUniqueContainerPointers(manifolds_to_delete_.begin(), manifolds_to_delete_.end()); @@ -364,45 +359,15 @@ InternalAddParameterBlock(values, size); } -void ProblemImpl::InternalSetParameterization( - double* values, - ParameterBlock* parameter_block, - LocalParameterization* local_parameterization) { - parameter_block_to_local_param_[values] = local_parameterization; - Manifold* manifold = nullptr; - if (local_parameterization != nullptr) { - if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { - local_parameterizations_to_delete_.push_back(local_parameterization); - } - - manifold = new ManifoldAdapter(local_parameterization); - // Add the manifold to manifolds_to_delete_ unconditionally since - // we own it and it will need to be deleted. - manifolds_to_delete_.push_back(manifold); - } - - parameter_block->SetManifold(manifold); -} - void ProblemImpl::InternalSetManifold(double* values, ParameterBlock* parameter_block, Manifold* manifold) { - // Reset any association between this parameter block and a local - // parameterization. This only needs done while we are in the transition from - // LocalParameterization to Manifold. - parameter_block_to_local_param_[values] = nullptr; if (manifold != nullptr && options_.manifold_ownership == TAKE_OWNERSHIP) { manifolds_to_delete_.push_back(manifold); } parameter_block->SetManifold(manifold); } -void ProblemImpl::AddParameterBlock( - double* values, int size, LocalParameterization* local_parameterization) { - ParameterBlock* parameter_block = InternalAddParameterBlock(values, size); - InternalSetParameterization(values, parameter_block, local_parameterization); -} - void ProblemImpl::AddParameterBlock(double* values, int size, Manifold* manifold) { @@ -538,19 +503,6 @@ parameter_block->SetVarying(); } -void ProblemImpl::SetParameterization( - double* values, LocalParameterization* local_parameterization) { - ParameterBlock* parameter_block = - FindWithDefault(parameter_block_map_, values, nullptr); - if (parameter_block == nullptr) { - LOG(FATAL) << "Parameter block not found: " << values - << ". You must add the parameter block to the problem before " - << "you can set its local parameterization."; - } - - InternalSetParameterization(values, parameter_block, local_parameterization); -} - void ProblemImpl::SetManifold(double* values, Manifold* manifold) { ParameterBlock* parameter_block = FindWithDefault(parameter_block_map_, values, nullptr); @@ -563,22 +515,13 @@ InternalSetManifold(values, parameter_block, manifold); } -const LocalParameterization* ProblemImpl::GetParameterization( - const double* values) const { - return FindWithDefault(parameter_block_to_local_param_, values, nullptr); -} - -bool ProblemImpl::HasParameterization(const double* values) const { - return GetParameterization(values) != nullptr; -} - const Manifold* ProblemImpl::GetManifold(const double* values) const { ParameterBlock* parameter_block = FindWithDefault( parameter_block_map_, const_cast<double*>(values), nullptr); if (parameter_block == nullptr) { LOG(FATAL) << "Parameter block not found: " << values << ". You must add the parameter block to the problem before " - << "you can get its local parameterization."; + << "you can get its manifold."; } return parameter_block->manifold();
diff --git a/internal/ceres/problem_impl.h b/internal/ceres/problem_impl.h index 22073b6..0ef3497 100644 --- a/internal/ceres/problem_impl.h +++ b/internal/ceres/problem_impl.h
@@ -59,7 +59,6 @@ class CostFunction; class EvaluationCallback; class LossFunction; -class LocalParameterization; struct CRSMatrix; namespace internal { @@ -100,10 +99,6 @@ } void AddParameterBlock(double* values, int size); - void AddParameterBlock(double* values, - int size, - LocalParameterization* local_parameterization); - void AddParameterBlock(double* values, int size, Manifold* manifold); void RemoveResidualBlock(ResidualBlock* residual_block); @@ -113,11 +108,6 @@ void SetParameterBlockVariable(double* values); bool IsParameterBlockConstant(const double* values) const; - void SetParameterization(double* values, - LocalParameterization* local_parameterization); - const LocalParameterization* GetParameterization(const double* values) const; - bool HasParameterization(const double* values) const; - void SetManifold(double* values, Manifold* manifold); const Manifold* GetManifold(const double* values) const; bool HasManifold(const double* values) const; @@ -180,10 +170,6 @@ private: ParameterBlock* InternalAddParameterBlock(double* values, int size); - void InternalSetParameterization( - double* values, - ParameterBlock* parameter_block, - LocalParameterization* local_parameterization); void InternalSetManifold(double* values, ParameterBlock* parameter_block, Manifold* manifold); @@ -218,12 +204,6 @@ // objects and reference counting for CostFunctions and LossFunctions. Ideally // this should be done uniformly. - // When removing parameter blocks, parameterizations have ambiguous - // ownership. Instead of scanning the entire problem to see if the - // parameterization is shared with other parameter blocks, buffer - // them until destruction. - std::vector<LocalParameterization*> local_parameterizations_to_delete_; - // When removing parameter blocks, manifolds have ambiguous // ownership. Instead of scanning the entire problem to see if the // manifold is shared with other parameter blocks, buffer @@ -236,17 +216,6 @@ // destroyed. CostFunctionRefCount cost_function_ref_count_; LossFunctionRefCount loss_function_ref_count_; - - // Because we wrap LocalParameterization objects using a ManifoldAdapter, when - // the user calls GetParameterization we cannot use the same logic as - // GetManifold as the ParameterBlock object only returns a Manifold object. So - // this map stores the association between parameter blocks and local - // parameterizations. - // - // This is a temporary object which will be removed once the - // LocalParameterization to Manifold transition is complete. - std::unordered_map<const double*, LocalParameterization*> - parameter_block_to_local_param_; }; } // namespace internal
diff --git a/internal/ceres/problem_test.cc b/internal/ceres/problem_test.cc index d7ec1f9..eb5a86f 100644 --- a/internal/ceres/problem_test.cc +++ b/internal/ceres/problem_test.cc
@@ -39,7 +39,6 @@ #include "ceres/crs_matrix.h" #include "ceres/evaluator_test_utils.h" #include "ceres/internal/eigen.h" -#include "ceres/local_parameterization.h" #include "ceres/loss_function.h" #include "ceres/map_util.h" #include "ceres/parameter_block.h" @@ -289,43 +288,6 @@ EXPECT_EQ(7, problem.NumParameters()); } -TEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) { - double x[3], y[4], z[5], w[4]; - - Problem problem; - problem.AddParameterBlock(x, 3); - EXPECT_EQ(1, problem.NumParameterBlocks()); - EXPECT_EQ(3, problem.NumParameters()); - - problem.AddParameterBlock(y, 4); - EXPECT_EQ(2, problem.NumParameterBlocks()); - EXPECT_EQ(7, problem.NumParameters()); - - problem.AddParameterBlock(z, 5); - EXPECT_EQ(3, problem.NumParameterBlocks()); - EXPECT_EQ(12, problem.NumParameters()); - - // Add a parameter that has a local parameterization. - w[0] = 1.0; - w[1] = 0.0; - w[2] = 0.0; - w[3] = 0.0; - problem.AddParameterBlock(w, 4, new QuaternionParameterization); - EXPECT_EQ(4, problem.NumParameterBlocks()); - EXPECT_EQ(16, problem.NumParameters()); - - problem.AddResidualBlock(new UnaryCostFunction(2, 3), nullptr, x); - problem.AddResidualBlock(new BinaryCostFunction(6, 5, 4), nullptr, z, y); - problem.AddResidualBlock(new BinaryCostFunction(3, 3, 5), nullptr, x, z); - problem.AddResidualBlock(new BinaryCostFunction(7, 5, 3), nullptr, z, x); - problem.AddResidualBlock( - new TernaryCostFunction(1, 5, 3, 4), nullptr, z, x, y); - - const int total_residuals = 2 + 6 + 3 + 7 + 1; - EXPECT_EQ(problem.NumResidualBlocks(), 5); - EXPECT_EQ(problem.NumResiduals(), total_residuals); -} - class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> { public: explicit DestructorCountingCostFunction(int* num_destructions) @@ -574,18 +536,6 @@ "Parameter block not found:"); } -TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) { - double x[3]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 3); - - EXPECT_DEATH_IF_SUPPORTED( - problem.SetParameterization(y, new IdentityParameterization(3)), - "Parameter block not found:"); -} - TEST(Problem, SetManifoldWithUnknownPtrDies) { double x[3]; double y[2]; @@ -608,20 +558,6 @@ "Parameter block not found:"); } -TEST(Problem, GetParameterization) { - double x[3]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 3); - problem.AddParameterBlock(y, 2); - - LocalParameterization* parameterization = new IdentityParameterization(3); - problem.SetParameterization(x, parameterization); - EXPECT_EQ(problem.GetParameterization(x), parameterization); - EXPECT_TRUE(problem.GetParameterization(y) == nullptr); -} - TEST(Problem, GetManifold) { double x[3]; double y[2]; @@ -636,20 +572,6 @@ EXPECT_TRUE(problem.GetManifold(y) == nullptr); } -TEST(Problem, HasParameterization) { - double x[3]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 3); - problem.AddParameterBlock(y, 2); - - LocalParameterization* parameterization = new IdentityParameterization(3); - problem.SetParameterization(x, parameterization); - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_FALSE(problem.HasParameterization(y)); -} - TEST(Problem, HasManifold) { double x[3]; double y[2]; @@ -664,53 +586,6 @@ EXPECT_FALSE(problem.HasManifold(y)); } -TEST(Problem, RepeatedAddParameterBlockResetsParameterization) { - double x[4]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 4, new SubsetParameterization(4, {0, 1})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 4); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2); - - problem.AddParameterBlock(x, 4, static_cast<LocalParameterization*>(nullptr)); - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_FALSE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 4); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 4); - EXPECT_EQ(problem.GetManifold(x), nullptr); - EXPECT_EQ(problem.GetParameterization(x), nullptr); - - problem.AddParameterBlock(x, 4, new SubsetParameterization(4, {0, 1, 2})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 1); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 1); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 1); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 4); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 1); -} - TEST(Problem, RepeatedAddParameterBlockResetsManifold) { double x[4]; double y[2]; @@ -721,235 +596,27 @@ EXPECT_FALSE(problem.HasManifold(y)); - EXPECT_FALSE(problem.HasParameterization(x)); EXPECT_TRUE(problem.HasManifold(x)); EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); problem.AddParameterBlock(x, 4, static_cast<Manifold*>(nullptr)); - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_FALSE(problem.HasParameterization(x)); EXPECT_FALSE(problem.HasManifold(x)); EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 4); EXPECT_EQ(problem.ParameterBlockTangentSize(x), 4); EXPECT_EQ(problem.GetManifold(x), nullptr); - EXPECT_EQ(problem.GetParameterization(x), nullptr); problem.AddParameterBlock(x, 4, new SubsetManifold(4, {0, 1, 2})); problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_FALSE(problem.HasParameterization(x)); EXPECT_TRUE(problem.HasManifold(x)); EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 1); EXPECT_EQ(problem.ParameterBlockTangentSize(x), 1); EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 1); } -TEST(Problem, AddParameterBlockWithManifoldNullLocalParameterization) { - double x[4]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 4, new SubsetManifold(4, {0, 1})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasManifold(y)); - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); - - problem.AddParameterBlock(x, 4, static_cast<LocalParameterization*>(nullptr)); - EXPECT_FALSE(problem.HasParameterization(y)); - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_FALSE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 4); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 4); - EXPECT_EQ(problem.GetManifold(x), nullptr); - EXPECT_EQ(problem.GetParameterization(x), nullptr); -} - -TEST(Problem, AddParameterBlockWithLocalParameterizationNullManifold) { - double x[4]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 4, new SubsetParameterization(4, {0, 1})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasManifold(y)); - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); - - problem.AddParameterBlock(x, 4, static_cast<Manifold*>(nullptr)); - EXPECT_FALSE(problem.HasParameterization(y)); - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_FALSE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 4); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 4); - EXPECT_EQ(problem.GetManifold(x), nullptr); - EXPECT_EQ(problem.GetParameterization(x), nullptr); -} - -TEST(Problem, RepeatedAddParameterBlockManifoldThenLocalParameterization) { - double x[4]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 4, new SubsetManifold(4, {0, 1})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasManifold(y)); - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); - - problem.AddParameterBlock(x, 4, new SubsetParameterization(4, {0, 1, 2})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 1); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 1); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 1); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 4); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 1); -} - -TEST(Problem, RepeatedAddParameterBlockLocalParameterizationThenManifold) { - double x[4]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 4, new SubsetParameterization(4, {0, 1, 2})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 1); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 1); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 1); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 4); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 1); - - problem.AddParameterBlock(x, 4, new SubsetManifold(4, {0, 1})); - problem.AddParameterBlock(y, 2); - - EXPECT_FALSE(problem.HasManifold(y)); - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); -} - -TEST(Problem, ParameterizationAndManifoldSittingInATree) { - double x[4]; - double y[2]; - - Problem problem; - problem.AddParameterBlock(x, 4); - problem.AddParameterBlock(y, 2); - - problem.SetParameterization(x, new SubsetParameterization(4, {0, 1})); - - EXPECT_FALSE(problem.HasParameterization(y)); - - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 4); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2); - - problem.SetManifold(x, new SubsetManifold(4, {1, 2, 3})); - EXPECT_FALSE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 1); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 1); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 1); - EXPECT_EQ(problem.GetParameterization(x), nullptr); - - problem.SetParameterization(x, new SubsetParameterization(4, {0, 1})); - EXPECT_TRUE(problem.HasParameterization(x)); - EXPECT_TRUE(problem.HasManifold(x)); - EXPECT_EQ(problem.ParameterBlockSize(x), 4); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); - EXPECT_EQ(problem.GetManifold(x)->AmbientSize(), 4); - EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 2); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 4); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2); -}; - -TEST(Problem, ParameterBlockQueryTestUsingLocalParameterization) { - double x[3]; - double y[4]; - Problem problem; - problem.AddParameterBlock(x, 3); - problem.AddParameterBlock(y, 4); - - vector<int> constant_parameters; - constant_parameters.push_back(0); - problem.SetParameterization( - x, new SubsetParameterization(3, constant_parameters)); - EXPECT_EQ(problem.ParameterBlockSize(x), 3); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4); - - vector<double*> parameter_blocks; - problem.GetParameterBlocks(¶meter_blocks); - EXPECT_EQ(parameter_blocks.size(), 2); - EXPECT_NE(parameter_blocks[0], parameter_blocks[1]); - EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y); - EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y); - - EXPECT_TRUE(problem.HasParameterBlock(x)); - problem.RemoveParameterBlock(x); - EXPECT_FALSE(problem.HasParameterBlock(x)); - problem.GetParameterBlocks(¶meter_blocks); - EXPECT_EQ(parameter_blocks.size(), 1); - EXPECT_TRUE(parameter_blocks[0] == y); -} - TEST(Problem, ParameterBlockQueryTestUsingManifold) { double x[3]; double y[4]; @@ -961,8 +628,8 @@ constant_parameters.push_back(0); problem.SetManifold(x, new SubsetManifold(3, constant_parameters)); EXPECT_EQ(problem.ParameterBlockSize(x), 3); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4); + EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); + EXPECT_EQ(problem.ParameterBlockTangentSize(y), 4); vector<double*> parameter_blocks; problem.GetParameterBlocks(¶meter_blocks); @@ -990,8 +657,8 @@ constant_parameters.push_back(0); problem.SetManifold(x, new SubsetManifold(3, constant_parameters)); EXPECT_EQ(problem.ParameterBlockSize(x), 3); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); - EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4); + EXPECT_EQ(problem.ParameterBlockTangentSize(x), 2); + EXPECT_EQ(problem.ParameterBlockTangentSize(y), 4); vector<double*> parameter_blocks; problem.GetParameterBlocks(¶meter_blocks); @@ -1895,43 +1562,6 @@ CheckAllEvaluationCombinations(evaluate_options, expected); } -TEST_F(ProblemEvaluateTest, LocalParameterization) { - // clang-format off - ExpectedEvaluation expected = { - // Rows/columns - 6, 5, - // Cost - 7607.0, - // Residuals - { -19.0, -35.0, // f - -59.0, -87.0, // g - -27.0, -43.0 // h - }, - // Gradient - { 146.0, 484.0, // x - 1256.0, // y with SubsetParameterization - 1450.0, 2604.0, // z - }, - // Jacobian - // x y z - { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, - 0.0, -4.0, -16.0, 0.0, 0.0, - /* g(y, z) */ 0.0, 0.0, 0.0, -20.0, 0.0, - 0.0, 0.0, -8.0, 0.0, -24.0, - /* h(z, x) */ -4.0, 0.0, 0.0, -10.0, 0.0, - 0.0, -8.0, 0.0, 0.0, -12.0 - } - }; - // clang-format on - - vector<int> constant_parameters; - constant_parameters.push_back(0); - problem_.SetParameterization( - parameters_ + 2, new SubsetParameterization(2, constant_parameters)); - - CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); -} - TEST_F(ProblemEvaluateTest, Manifold) { // clang-format off ExpectedEvaluation expected = { @@ -1946,7 +1576,7 @@ }, // Gradient { 146.0, 484.0, // x - 1256.0, // y with SubsetParameterization + 1256.0, // y with SubsetManifold 1450.0, 2604.0, // z }, // Jacobian @@ -2251,50 +1881,6 @@ << actual_dfdy; } -TEST_F(ProblemEvaluateResidualBlockTest, - OneResidualBlockWithOneLocalParameterization) { - ResidualBlockId residual_block_id = - problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_); - problem_.SetParameterization(x_, new SubsetParameterization(2, {1})); - - Vector expected_f(5); - expected_f << 1, 2, 1, 2, 3; - Matrix expected_dfdx = Matrix::Zero(5, 1); - expected_dfdx.block(0, 0, 1, 1) = Matrix::Identity(1, 1); - Matrix expected_dfdy = Matrix::Zero(5, 3); - expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3); - double expected_cost = expected_f.squaredNorm() / 2.0; - - double actual_cost; - Vector actual_f(5); - Matrix actual_dfdx(5, 1); - Matrix actual_dfdy(5, 3); - double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()}; - EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id, - kApplyLossFunction, - kNewPoint, - &actual_cost, - actual_f.data(), - jacobians)); - - EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost, - 0, - std::numeric_limits<double>::epsilon()) - << actual_cost; - EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(), - 0, - std::numeric_limits<double>::epsilon()) - << actual_f; - EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(), - 0, - std::numeric_limits<double>::epsilon()) - << actual_dfdx; - EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(), - 0, - std::numeric_limits<double>::epsilon()) - << actual_dfdy; -} - TEST_F(ProblemEvaluateResidualBlockTest, OneResidualBlockWithOneManifold) { ResidualBlockId residual_block_id = problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_); @@ -2338,51 +1924,6 @@ << actual_dfdy; } -TEST_F(ProblemEvaluateResidualBlockTest, - OneResidualBlockWithTwoLocalParameterizations) { - ResidualBlockId residual_block_id = - problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_); - problem_.SetParameterization(x_, new SubsetParameterization(2, {1})); - problem_.SetParameterization(y_, new SubsetParameterization(3, {2})); - - Vector expected_f(5); - expected_f << 1, 2, 1, 2, 3; - Matrix expected_dfdx = Matrix::Zero(5, 1); - expected_dfdx.block(0, 0, 1, 1) = Matrix::Identity(1, 1); - Matrix expected_dfdy = Matrix::Zero(5, 2); - expected_dfdy.block(2, 0, 2, 2) = Matrix::Identity(2, 2); - double expected_cost = expected_f.squaredNorm() / 2.0; - - double actual_cost; - Vector actual_f(5); - Matrix actual_dfdx(5, 1); - Matrix actual_dfdy(5, 2); - double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()}; - EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id, - kApplyLossFunction, - kNewPoint, - &actual_cost, - actual_f.data(), - jacobians)); - - EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost, - 0, - std::numeric_limits<double>::epsilon()) - << actual_cost; - EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(), - 0, - std::numeric_limits<double>::epsilon()) - << actual_f; - EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(), - 0, - std::numeric_limits<double>::epsilon()) - << actual_dfdx; - EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(), - 0, - std::numeric_limits<double>::epsilon()) - << actual_dfdy; -} - TEST_F(ProblemEvaluateResidualBlockTest, OneResidualBlockWithTwoManifolds) { ResidualBlockId residual_block_id = problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_); @@ -2628,19 +2169,6 @@ std::numeric_limits<double>::max()); } -TEST(Problem, SetParameterizationTwice) { - Problem problem; - double x[] = {1.0, 2.0, 3.0}; - problem.AddParameterBlock(x, 3); - problem.SetParameterization(x, new SubsetParameterization(3, {1})); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2); - - problem.SetParameterization(x, new SubsetParameterization(3, {0, 1})); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 1); -} - TEST(Problem, SetManifoldTwice) { Problem problem; double x[] = {1.0, 2.0, 3.0}; @@ -2654,20 +2182,6 @@ EXPECT_EQ(problem.GetManifold(x)->TangentSize(), 1); } -TEST(Problem, SetParameterizationAndThenClearItWithNull) { - Problem problem; - double x[] = {1.0, 2.0, 3.0}; - problem.AddParameterBlock(x, 3); - problem.SetParameterization(x, new SubsetParameterization(3, {1})); - EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3); - EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2); - - problem.SetParameterization(x, nullptr); - EXPECT_EQ(problem.GetParameterization(x), nullptr); - EXPECT_EQ(problem.ParameterBlockLocalSize(x), 3); - EXPECT_EQ(problem.ParameterBlockSize(x), 3); -} - TEST(Problem, SetManifoldAndThenClearItWithNull) { Problem problem; double x[] = {1.0, 2.0, 3.0}; @@ -2682,15 +2196,6 @@ EXPECT_EQ(problem.ParameterBlockSize(x), 3); } -TEST(Solver, ZeroSizedLocalParameterizationMeansParameterBlockIsConstant) { - double x = 0.0; - double y = 1.0; - Problem problem; - problem.AddResidualBlock(new BinaryCostFunction(1, 1, 1), nullptr, &x, &y); - problem.SetParameterization(&y, new SubsetParameterization(1, {0})); - EXPECT_TRUE(problem.IsParameterBlockConstant(&y)); -} - TEST(Solver, ZeroTangentSizedManifoldMeansParameterBlockIsConstant) { double x = 0.0; double y = 1.0;
diff --git a/internal/ceres/solver_test.cc b/internal/ceres/solver_test.cc index e73e899..5abd723 100644 --- a/internal/ceres/solver_test.cc +++ b/internal/ceres/solver_test.cc
@@ -37,7 +37,6 @@ #include "ceres/autodiff_cost_function.h" #include "ceres/evaluation_callback.h" -#include "ceres/local_parameterization.h" #include "ceres/manifold.h" #include "ceres/problem.h" #include "ceres/problem_impl.h" @@ -510,26 +509,6 @@ } }; -TEST(Solver, ZeroSizedLocalParameterizationHoldsParameterBlockConstant) { - double x = 0.0; - double y = 1.0; - Problem problem; - problem.AddResidualBlock(LinearCostFunction::Create(), nullptr, &x, &y); - problem.SetParameterization(&y, new SubsetParameterization(1, {0})); - EXPECT_TRUE(problem.IsParameterBlockConstant(&y)); - - Solver::Options options; - options.function_tolerance = 0.0; - options.gradient_tolerance = 0.0; - options.parameter_tolerance = 0.0; - Solver::Summary summary; - Solve(options, &problem, &summary); - - EXPECT_EQ(summary.termination_type, CONVERGENCE); - EXPECT_NEAR(x, 10.0, 1e-7); - EXPECT_EQ(y, 1.0); -} - TEST(Solver, ZeroSizedManifoldHoldsParameterBlockConstant) { double x = 0.0; double y = 1.0;