Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/extern/ceres/internal/ceres/manifold.cc
diff options
context:
space:
mode:
Diffstat (limited to 'extern/ceres/internal/ceres/manifold.cc')
-rw-r--r--extern/ceres/internal/ceres/manifold.cc321
1 files changed, 321 insertions, 0 deletions
diff --git a/extern/ceres/internal/ceres/manifold.cc b/extern/ceres/internal/ceres/manifold.cc
new file mode 100644
index 00000000000..f412a793f93
--- /dev/null
+++ b/extern/ceres/internal/ceres/manifold.cc
@@ -0,0 +1,321 @@
+#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
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-26 10:20:55 +0300