Something went wrong. Try again.
The open source OpenXR runtime
Something went wrong. Try again.
12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182// Copyright 2026, Beyley Cardellio// SPDX-License-Identifier: BSL-1.0/*! * @file * @brief Generic math helpers for constellation optimizer code. * @author Beyley Cardellio <ep1cm1n10n123@gmail.com> * @ingroup tracking */
#pragma once
#include "math/m_eigen_interop.hpp"#include "math/m_quatexpmap.hpp"#include "math/m_quatexpmap_bigceres.hpp"
#include <Eigen/Core>#include <Eigen/Geometry>
#include <ceres/jet.h>#include <ceres/product_manifold.h>#include <ceres/autodiff_manifold.h>
#include "pose_optimize.hpp"
namespace xrt::tracking::constellation::optimizer {
using namespace xrt::auxiliary::math;
// Covariance lives in the manifold's tangent space, which is one dimension smaller than the ambient// parameter block: the quaternion contributes 3 tangent dimensions, not 4.typedef Eigen::Matrix<double, kPoseCovarianceSize, kPoseCovarianceSize> PoseStateCovarianceMatrix;
//! Returns an Isometry3f from an @ref xrt_pose.inline Eigen::Isometry3fisometryFromPose(const xrt_pose &p){ return {Eigen::Translation3f{map_vec3(p.position)} * map_quat(p.orientation)};}
/*! * Right-multiplying quaternion manifold, matches our local convention. */struct QuaternionManifoldFunctor{ template <typename T> bool Plus(const T *x, const T *delta, T *x_plus_delta) const { const Eigen::Map<const Eigen::Quaternion<T>> q(x); const Eigen::Map<const Eigen::Vector3<T>> w(delta); Eigen::Map<Eigen::Quaternion<T>> result(x_plus_delta);
result = q * quat_exp_so3(w); return true; }
template <typename T> bool Minus(const T *y, const T *x, T *y_minus_x) const { const Eigen::Map<const Eigen::Quaternion<T>> q_y(y); const Eigen::Map<const Eigen::Quaternion<T>> q_x(x); Eigen::Map<Eigen::Vector3<T>> result(y_minus_x);
result = quat_ln_so3(Eigen::Quaternion<T>(q_x.conjugate() * q_y)); return true; }};
// Pose manifold, (tX, tY, tZ), (qX, qY, qZ, qW)typedef ceres::ProductManifold<ceres::EuclideanManifold<3>, ceres::AutoDiffManifold<QuaternionManifoldFunctor, 4, 3>> PoseManifold;
// Pose plus linear velocity, (tX, tY, tZ), (qX, qY, qZ, qW), (vX, vY, vZ). 10 ambient, 9 tangent.typedef ceres::ProductManifold<ceres::EuclideanManifold<3>, ceres::AutoDiffManifold<QuaternionManifoldFunctor, 4, 3>, ceres::EuclideanManifold<3>> PoseWithVelocityManifold;
}; // namespace xrt::tracking::constellation::optimizer