You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
320 lines
12 KiB
320 lines
12 KiB
1 month ago
|
// 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_
|