blob: ed2d1f437405d064ba63c95fde46f39a5c32fcbd [file] [log] [blame]
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
// sergiu.deitsch@gmail.com (Sergiu Deitsch)
//
#ifndef CERES_PUBLIC_PRODUCT_MANIFOLD_H_
#define CERES_PUBLIC_PRODUCT_MANIFOLD_H_
#include <algorithm>
#include <array>
#include <cassert>
#include <cstddef>
#include <numeric>
#include <tuple>
#include <type_traits>
#include <utility>
#include "ceres/internal/eigen.h"
#include "ceres/internal/fixed_array.h"
#include "ceres/internal/port.h"
#include "ceres/manifold.h"
namespace ceres {
// Construct a manifold by taking the Cartesian product of a number of other
// manifolds. 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:
//
// ProductManifold<QuaternionManifold, EuclideanManifold<3>> se3;
//
// is the manifold for a rigid transformation, where the rotation is
// represented using a quaternion.
//
// Manifolds can be copied and moved to ProductManifold:
//
// SubsetManifold manifold1(5, {2});
// SubsetManifold manifold2(3, {0, 1});
// ProductManifold<SubsetManifold, SubsetManifold> manifold(manifold1,
// manifold2);
//
// In advanced use cases, manifolds can be dynamically allocated and passed as
// (smart) pointers:
//
// ProductManifold<std::unique_ptr<QuaternionManifold>, EuclideanManifold<3>>
// se3{std::make_unique<QuaternionManifold>(), EuclideanManifold<3>{}};
//
// In C++17, the template parameters can be left out as they are automatically
// deduced making the initialization much simpler:
//
// ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}};
//
// The manifold implementations must be either default constructible, copyable
// or moveable to be usable in a ProductManifold.
template <typename Manifold0, typename Manifold1, typename... ManifoldN>
class ProductManifold final : public Manifold {
public:
// ProductManifold constructor perfect forwards arguments to store manifolds.
//
// Either use default construction or if you need to copy or move-construct a
// manifold instance, you need to pass an instance as an argument for all
// types given as class template parameters.
template <typename... Args,
std::enable_if_t<std::is_constructible<
std::tuple<Manifold0, Manifold1, ManifoldN...>,
Args...>::value>* = nullptr>
explicit ProductManifold(Args&&... manifolds)
: ProductManifold{std::make_index_sequence<kNumManifolds>{},
std::forward<Args>(manifolds)...} {}
int AmbientSize() const override { return ambient_size_; }
int TangentSize() const override { return tangent_size_; }
bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const override {
return PlusImpl(
x, delta, x_plus_delta, std::make_index_sequence<kNumManifolds>{});
}
bool Minus(const double* y,
const double* x,
double* y_minus_x) const override {
return MinusImpl(
y, x, y_minus_x, std::make_index_sequence<kNumManifolds>{});
}
bool PlusJacobian(const double* x, double* jacobian_ptr) const override {
MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize());
jacobian.setZero();
internal::FixedArray<double> buffer(buffer_size_);
return PlusJacobianImpl(
x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{});
}
bool MinusJacobian(const double* x, double* jacobian_ptr) const override {
MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize());
jacobian.setZero();
internal::FixedArray<double> buffer(buffer_size_);
return MinusJacobianImpl(
x, jacobian, buffer, std::make_index_sequence<kNumManifolds>{});
}
private:
static constexpr std::size_t kNumManifolds = 2 + sizeof...(ManifoldN);
template <std::size_t... Indices, typename... Args>
explicit ProductManifold(std::index_sequence<Indices...>, Args&&... manifolds)
: manifolds_{std::forward<Args>(manifolds)...},
buffer_size_{(std::max)(
{(Dereference(std::get<Indices>(manifolds_)).TangentSize() *
Dereference(std::get<Indices>(manifolds_)).AmbientSize())...})},
ambient_sizes_{
Dereference(std::get<Indices>(manifolds_)).AmbientSize()...},
tangent_sizes_{
Dereference(std::get<Indices>(manifolds_)).TangentSize()...},
ambient_offsets_{ExclusiveScan(ambient_sizes_)},
tangent_offsets_{ExclusiveScan(tangent_sizes_)},
ambient_size_{
std::accumulate(ambient_sizes_.begin(), ambient_sizes_.end(), 0)},
tangent_size_{
std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)} {}
template <std::size_t Index0, std::size_t... Indices>
bool PlusImpl(const double* x,
const double* delta,
double* x_plus_delta,
std::index_sequence<Index0, Indices...>) const {
if (!Dereference(std::get<Index0>(manifolds_))
.Plus(x + ambient_offsets_[Index0],
delta + tangent_offsets_[Index0],
x_plus_delta + ambient_offsets_[Index0])) {
return false;
}
return PlusImpl(x, delta, x_plus_delta, std::index_sequence<Indices...>{});
}
static constexpr bool PlusImpl(const double* /*x*/,
const double* /*delta*/,
double* /*x_plus_delta*/,
std::index_sequence<>) noexcept {
return true;
}
template <std::size_t Index0, std::size_t... Indices>
bool MinusImpl(const double* y,
const double* x,
double* y_minus_x,
std::index_sequence<Index0, Indices...>) const {
if (!Dereference(std::get<Index0>(manifolds_))
.Minus(y + ambient_offsets_[Index0],
x + ambient_offsets_[Index0],
y_minus_x + tangent_offsets_[Index0])) {
return false;
}
return MinusImpl(y, x, y_minus_x, std::index_sequence<Indices...>{});
}
static constexpr bool MinusImpl(const double* /*y*/,
const double* /*x*/,
double* /*y_minus_x*/,
std::index_sequence<>) noexcept {
return true;
}
template <std::size_t Index0, std::size_t... Indices>
bool PlusJacobianImpl(const double* x,
MatrixRef& jacobian,
internal::FixedArray<double>& buffer,
std::index_sequence<Index0, Indices...>) const {
if (!Dereference(std::get<Index0>(manifolds_))
.PlusJacobian(x + ambient_offsets_[Index0], buffer.data())) {
return false;
}
jacobian.block(ambient_offsets_[Index0],
tangent_offsets_[Index0],
ambient_sizes_[Index0],
tangent_sizes_[Index0]) =
MatrixRef(
buffer.data(), ambient_sizes_[Index0], tangent_sizes_[Index0]);
return PlusJacobianImpl(
x, jacobian, buffer, std::index_sequence<Indices...>{});
}
static constexpr bool PlusJacobianImpl(
const double* /*x*/,
MatrixRef& /*jacobian*/,
internal::FixedArray<double>& /*buffer*/,
std::index_sequence<>) noexcept {
return true;
}
template <std::size_t Index0, std::size_t... Indices>
bool MinusJacobianImpl(const double* x,
MatrixRef& jacobian,
internal::FixedArray<double>& buffer,
std::index_sequence<Index0, Indices...>) const {
if (!Dereference(std::get<Index0>(manifolds_))
.MinusJacobian(x + ambient_offsets_[Index0], buffer.data())) {
return false;
}
jacobian.block(tangent_offsets_[Index0],
ambient_offsets_[Index0],
tangent_sizes_[Index0],
ambient_sizes_[Index0]) =
MatrixRef(
buffer.data(), tangent_sizes_[Index0], ambient_sizes_[Index0]);
return MinusJacobianImpl(
x, jacobian, buffer, std::index_sequence<Indices...>{});
}
static constexpr bool MinusJacobianImpl(
const double* /*x*/,
MatrixRef& /*jacobian*/,
internal::FixedArray<double>& /*buffer*/,
std::index_sequence<>) noexcept {
return true;
}
template <typename T, std::size_t N>
static std::array<T, N> ExclusiveScan(const std::array<T, N>& values) {
std::array<T, N> result;
// TODO Replace with std::exclusive_scan once all platforms have full C++17
// STL support.
T init = 0;
for (std::size_t i = 0; i != N; ++i) {
result[i] = init;
init += values[i];
}
return result;
}
template <typename T, typename E = void>
struct IsDereferenceable : std::false_type {};
template <typename T>
struct IsDereferenceable<T, std::void_t<decltype(*std::declval<T>())>>
: std::true_type {};
template <typename T,
std::enable_if_t<!IsDereferenceable<T>::value>* = nullptr>
static constexpr decltype(auto) Dereference(T& value) {
return value;
}
// Support dereferenceable types such as std::unique_ptr, std::shared_ptr, raw
// pointers etc.
template <typename T,
std::enable_if_t<IsDereferenceable<T>::value>* = nullptr>
static constexpr decltype(auto) Dereference(T& value) {
return *value;
}
template <typename T>
static constexpr decltype(auto) Dereference(T* p) {
assert(p != nullptr);
return *p;
}
std::tuple<Manifold0, Manifold1, ManifoldN...> manifolds_;
int buffer_size_;
std::array<int, kNumManifolds> ambient_sizes_;
std::array<int, kNumManifolds> tangent_sizes_;
std::array<int, kNumManifolds> ambient_offsets_;
std::array<int, kNumManifolds> tangent_offsets_;
int ambient_size_;
int tangent_size_;
};
// C++17 deduction guide that allows the user to avoid explicitly specifying
// the template parameters of ProductManifold. The class can instead be
// instantiated as follows:
//
// ProductManifold manifold{QuaternionManifold{}, EuclideanManifold<3>{}};
//
template <typename Manifold0, typename Manifold1, typename... Manifolds>
ProductManifold(Manifold0&&, Manifold1&&, Manifolds&&...)
-> ProductManifold<Manifold0, Manifold1, Manifolds...>;
} // namespace ceres
#endif // CERES_PUBLIC_PRODUCT_MANIFOLD_H_