mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 16:40:38 +08:00
Sergiu Deitsch
7743d2e73c
Since the number of manifolds used to initialize ProductManifold and their types are known at compile-time, it is possible to avoid storing pointers to the base class as required by a homogeneous, currently dynamically sized container. Instead, we can use std::tuple<> as a heterogenous container with the number of elements fixed at compile-time that allows us to store the concrete manifold realizations. The advantage of this approach is that we can bypass the vtable when iterating over each manifold within ProductManifold. The indirection is invoked only once while accessing the ProductManifoldImpl members. Additionally, potential dynamic memory allocations by a std::vector can be completely avoided. This makes the ProductManifold implementation more efficient both in memory and runtime. Change-Id: Ic71b0c175ab726f8992e9703f7666bca477baf19
322 lines
11 KiB
C++
322 lines
11 KiB
C++
#include "ceres/manifold.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
#include "ceres/internal/eigen.h"
|
|
#include "ceres/internal/fixed_array.h"
|
|
#include "glog/logging.h"
|
|
|
|
namespace ceres {
|
|
namespace {
|
|
|
|
struct CeresQuaternionOrder {
|
|
static constexpr int kW = 0;
|
|
static constexpr int kX = 1;
|
|
static constexpr int kY = 2;
|
|
static constexpr int kZ = 3;
|
|
};
|
|
|
|
struct EigenQuaternionOrder {
|
|
static constexpr int kW = 3;
|
|
static constexpr int kX = 0;
|
|
static constexpr int kY = 1;
|
|
static constexpr int kZ = 2;
|
|
};
|
|
|
|
template <typename Order>
|
|
inline void QuaternionPlusImpl(const double* x,
|
|
const double* delta,
|
|
double* x_plus_delta) {
|
|
// x_plus_delta = QuaternionProduct(q_delta, x), where q_delta is the
|
|
// quaternion constructed from delta.
|
|
const double norm_delta = std::sqrt(
|
|
delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
|
|
|
|
if (norm_delta == 0.0) {
|
|
for (int i = 0; i < 4; ++i) {
|
|
x_plus_delta[i] = x[i];
|
|
}
|
|
return;
|
|
}
|
|
|
|
const double sin_delta_by_delta = (std::sin(norm_delta) / norm_delta);
|
|
double q_delta[4];
|
|
q_delta[Order::kW] = std::cos(norm_delta);
|
|
q_delta[Order::kX] = sin_delta_by_delta * delta[0];
|
|
q_delta[Order::kY] = sin_delta_by_delta * delta[1];
|
|
q_delta[Order::kZ] = sin_delta_by_delta * delta[2];
|
|
|
|
x_plus_delta[Order::kW] =
|
|
q_delta[Order::kW] * x[Order::kW] - q_delta[Order::kX] * x[Order::kX] -
|
|
q_delta[Order::kY] * x[Order::kY] - q_delta[Order::kZ] * x[Order::kZ];
|
|
x_plus_delta[Order::kX] =
|
|
q_delta[Order::kW] * x[Order::kX] + q_delta[Order::kX] * x[Order::kW] +
|
|
q_delta[Order::kY] * x[Order::kZ] - q_delta[Order::kZ] * x[Order::kY];
|
|
x_plus_delta[Order::kY] =
|
|
q_delta[Order::kW] * x[Order::kY] - q_delta[Order::kX] * x[Order::kZ] +
|
|
q_delta[Order::kY] * x[Order::kW] + q_delta[Order::kZ] * x[Order::kX];
|
|
x_plus_delta[Order::kZ] =
|
|
q_delta[Order::kW] * x[Order::kZ] + q_delta[Order::kX] * x[Order::kY] -
|
|
q_delta[Order::kY] * x[Order::kX] + q_delta[Order::kZ] * x[Order::kW];
|
|
}
|
|
|
|
template <typename Order>
|
|
inline void QuaternionPlusJacobianImpl(const double* x, double* jacobian_ptr) {
|
|
Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> jacobian(
|
|
jacobian_ptr);
|
|
|
|
jacobian(Order::kW, 0) = -x[Order::kX];
|
|
jacobian(Order::kW, 1) = -x[Order::kY];
|
|
jacobian(Order::kW, 2) = -x[Order::kZ];
|
|
jacobian(Order::kX, 0) = x[Order::kW];
|
|
jacobian(Order::kX, 1) = x[Order::kZ];
|
|
jacobian(Order::kX, 2) = -x[Order::kY];
|
|
jacobian(Order::kY, 0) = -x[Order::kZ];
|
|
jacobian(Order::kY, 1) = x[Order::kW];
|
|
jacobian(Order::kY, 2) = x[Order::kX];
|
|
jacobian(Order::kZ, 0) = x[Order::kY];
|
|
jacobian(Order::kZ, 1) = -x[Order::kX];
|
|
jacobian(Order::kZ, 2) = x[Order::kW];
|
|
}
|
|
|
|
template <typename Order>
|
|
inline void QuaternionMinusImpl(const double* y,
|
|
const double* x,
|
|
double* y_minus_x) {
|
|
// ambient_y_minus_x = QuaternionProduct(y, -x) where -x is the conjugate of
|
|
// x.
|
|
double ambient_y_minus_x[4];
|
|
ambient_y_minus_x[Order::kW] =
|
|
y[Order::kW] * x[Order::kW] + y[Order::kX] * x[Order::kX] +
|
|
y[Order::kY] * x[Order::kY] + y[Order::kZ] * x[Order::kZ];
|
|
ambient_y_minus_x[Order::kX] =
|
|
-y[Order::kW] * x[Order::kX] + y[Order::kX] * x[Order::kW] -
|
|
y[Order::kY] * x[Order::kZ] + y[Order::kZ] * x[Order::kY];
|
|
ambient_y_minus_x[Order::kY] =
|
|
-y[Order::kW] * x[Order::kY] + y[Order::kX] * x[Order::kZ] +
|
|
y[Order::kY] * x[Order::kW] - y[Order::kZ] * x[Order::kX];
|
|
ambient_y_minus_x[Order::kZ] =
|
|
-y[Order::kW] * x[Order::kZ] - y[Order::kX] * x[Order::kY] +
|
|
y[Order::kY] * x[Order::kX] + y[Order::kZ] * x[Order::kW];
|
|
|
|
const double u_norm =
|
|
std::sqrt(ambient_y_minus_x[Order::kX] * ambient_y_minus_x[Order::kX] +
|
|
ambient_y_minus_x[Order::kY] * ambient_y_minus_x[Order::kY] +
|
|
ambient_y_minus_x[Order::kZ] * ambient_y_minus_x[Order::kZ]);
|
|
if (u_norm > 0.0) {
|
|
const double theta = std::atan2(u_norm, ambient_y_minus_x[Order::kW]);
|
|
y_minus_x[0] = theta * ambient_y_minus_x[Order::kX] / u_norm;
|
|
y_minus_x[1] = theta * ambient_y_minus_x[Order::kY] / u_norm;
|
|
y_minus_x[2] = theta * ambient_y_minus_x[Order::kZ] / u_norm;
|
|
} else {
|
|
y_minus_x[0] = 0.0;
|
|
y_minus_x[1] = 0.0;
|
|
y_minus_x[2] = 0.0;
|
|
}
|
|
}
|
|
|
|
template <typename Order>
|
|
inline void QuaternionMinusJacobianImpl(const double* x, double* jacobian_ptr) {
|
|
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> jacobian(
|
|
jacobian_ptr);
|
|
|
|
jacobian(0, Order::kW) = -x[Order::kX];
|
|
jacobian(0, Order::kX) = x[Order::kW];
|
|
jacobian(0, Order::kY) = -x[Order::kZ];
|
|
jacobian(0, Order::kZ) = x[Order::kY];
|
|
jacobian(1, Order::kW) = -x[Order::kY];
|
|
jacobian(1, Order::kX) = x[Order::kZ];
|
|
jacobian(1, Order::kY) = x[Order::kW];
|
|
jacobian(1, Order::kZ) = -x[Order::kX];
|
|
jacobian(2, Order::kW) = -x[Order::kZ];
|
|
jacobian(2, Order::kX) = -x[Order::kY];
|
|
jacobian(2, Order::kY) = x[Order::kX];
|
|
jacobian(2, Order::kZ) = x[Order::kW];
|
|
}
|
|
|
|
} // namespace
|
|
|
|
Manifold::~Manifold() = default;
|
|
|
|
bool Manifold::RightMultiplyByPlusJacobian(const double* x,
|
|
const int num_rows,
|
|
const double* ambient_matrix,
|
|
double* tangent_matrix) const {
|
|
const int tangent_size = TangentSize();
|
|
if (tangent_size == 0) {
|
|
return true;
|
|
}
|
|
|
|
const int ambient_size = AmbientSize();
|
|
Matrix plus_jacobian(ambient_size, tangent_size);
|
|
if (!PlusJacobian(x, plus_jacobian.data())) {
|
|
return false;
|
|
}
|
|
|
|
MatrixRef(tangent_matrix, num_rows, tangent_size) =
|
|
ConstMatrixRef(ambient_matrix, num_rows, ambient_size) * plus_jacobian;
|
|
return true;
|
|
}
|
|
|
|
SubsetManifold::SubsetManifold(const int size,
|
|
const std::vector<int>& constant_parameters)
|
|
|
|
: tangent_size_(size - constant_parameters.size()),
|
|
constancy_mask_(size, false) {
|
|
if (constant_parameters.empty()) {
|
|
return;
|
|
}
|
|
|
|
std::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 (auto index : constant_parameters) {
|
|
constancy_mask_[index] = true;
|
|
}
|
|
}
|
|
|
|
int SubsetManifold::AmbientSize() const { return constancy_mask_.size(); }
|
|
|
|
int SubsetManifold::TangentSize() const { return tangent_size_; }
|
|
|
|
bool SubsetManifold::Plus(const double* x,
|
|
const double* delta,
|
|
double* x_plus_delta) const {
|
|
const int ambient_size = AmbientSize();
|
|
for (int i = 0, j = 0; i < ambient_size; ++i) {
|
|
if (constancy_mask_[i]) {
|
|
x_plus_delta[i] = x[i];
|
|
} else {
|
|
x_plus_delta[i] = x[i] + delta[j++];
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool SubsetManifold::PlusJacobian(const double* x,
|
|
double* plus_jacobian) const {
|
|
if (tangent_size_ == 0) {
|
|
return true;
|
|
}
|
|
|
|
const int ambient_size = AmbientSize();
|
|
MatrixRef m(plus_jacobian, ambient_size, tangent_size_);
|
|
m.setZero();
|
|
for (int r = 0, c = 0; r < ambient_size; ++r) {
|
|
if (!constancy_mask_[r]) {
|
|
m(r, c++) = 1.0;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool SubsetManifold::RightMultiplyByPlusJacobian(const double* x,
|
|
const int num_rows,
|
|
const double* ambient_matrix,
|
|
double* tangent_matrix) const {
|
|
if (tangent_size_ == 0) {
|
|
return true;
|
|
}
|
|
|
|
const int ambient_size = AmbientSize();
|
|
for (int r = 0; r < num_rows; ++r) {
|
|
for (int idx = 0, c = 0; idx < ambient_size; ++idx) {
|
|
if (!constancy_mask_[idx]) {
|
|
tangent_matrix[r * tangent_size_ + c++] =
|
|
ambient_matrix[r * ambient_size + idx];
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool SubsetManifold::Minus(const double* y,
|
|
const double* x,
|
|
double* y_minus_x) const {
|
|
if (tangent_size_ == 0) {
|
|
return true;
|
|
}
|
|
|
|
const int ambient_size = AmbientSize();
|
|
for (int i = 0, j = 0; i < ambient_size; ++i) {
|
|
if (!constancy_mask_[i]) {
|
|
y_minus_x[j++] = y[i] - x[i];
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool SubsetManifold::MinusJacobian(const double* x,
|
|
double* minus_jacobian) const {
|
|
const int ambient_size = AmbientSize();
|
|
MatrixRef m(minus_jacobian, tangent_size_, ambient_size);
|
|
m.setZero();
|
|
for (int c = 0, r = 0; c < ambient_size; ++c) {
|
|
if (!constancy_mask_[c]) {
|
|
m(r++, c) = 1.0;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool QuaternionManifold::Plus(const double* x,
|
|
const double* delta,
|
|
double* x_plus_delta) const {
|
|
QuaternionPlusImpl<CeresQuaternionOrder>(x, delta, x_plus_delta);
|
|
return true;
|
|
}
|
|
|
|
bool QuaternionManifold::PlusJacobian(const double* x, double* jacobian) const {
|
|
QuaternionPlusJacobianImpl<CeresQuaternionOrder>(x, jacobian);
|
|
return true;
|
|
}
|
|
|
|
bool QuaternionManifold::Minus(const double* y,
|
|
const double* x,
|
|
double* y_minus_x) const {
|
|
QuaternionMinusImpl<CeresQuaternionOrder>(y, x, y_minus_x);
|
|
return true;
|
|
}
|
|
|
|
bool QuaternionManifold::MinusJacobian(const double* x,
|
|
double* jacobian) const {
|
|
QuaternionMinusJacobianImpl<CeresQuaternionOrder>(x, jacobian);
|
|
return true;
|
|
}
|
|
|
|
bool EigenQuaternionManifold::Plus(const double* x,
|
|
const double* delta,
|
|
double* x_plus_delta) const {
|
|
QuaternionPlusImpl<EigenQuaternionOrder>(x, delta, x_plus_delta);
|
|
return true;
|
|
}
|
|
|
|
bool EigenQuaternionManifold::PlusJacobian(const double* x,
|
|
double* jacobian) const {
|
|
QuaternionPlusJacobianImpl<EigenQuaternionOrder>(x, jacobian);
|
|
return true;
|
|
}
|
|
|
|
bool EigenQuaternionManifold::Minus(const double* y,
|
|
const double* x,
|
|
double* y_minus_x) const {
|
|
QuaternionMinusImpl<EigenQuaternionOrder>(y, x, y_minus_x);
|
|
return true;
|
|
}
|
|
|
|
bool EigenQuaternionManifold::MinusJacobian(const double* x,
|
|
double* jacobian) const {
|
|
QuaternionMinusJacobianImpl<EigenQuaternionOrder>(x, jacobian);
|
|
return true;
|
|
}
|
|
|
|
} // namespace ceres
|