Monado OpenXR Runtime
Loading...
Searching...
No Matches
internal_math.hpp
Go to the documentation of this file.
1// Copyright 2026, Beyley Cardellio
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Generic math helpers for constellation optimizer code.
6 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
7 * @ingroup tracking
8 */
9
10#pragma once
11
13
15#include "math/m_quatexpmap.hpp"
17
18#include <Eigen/Core>
19#include <Eigen/Geometry>
20
21#include "ceres/jet.h"
22#include "ceres/product_manifold.h"
23#include "ceres/autodiff_manifold.h"
24#include "ceres/ceres.h"
25
26#include "pose_optimize.hpp"
27#include "math.hpp"
28
29#include <ceres/crs_matrix.h>
30#include <cstring>
31#include <cmath>
32
33
34namespace xrt::tracking::constellation::optimizer {
35
36/*!
37 * Optimize directly on the pre-undistorted points, don't compute residuals in distorted pixel space.
38 *
39 * We don't have a "project" routine for the CV1 distortion algorithm, so we work in undistorted space, instead.
40 */
41constexpr bool kOptimizeUndistortedPoints = true;
42//! Print debug info about the residuals as we're computing them.
43constexpr bool kResidualDebugPrint = false;
44
45constexpr double kMaxGravity = MATH_GRAVITY_M_S2 * 1.01;
46constexpr double kMinGravity = MATH_GRAVITY_M_S2 * 0.99;
47
48/*!
49 * Fewer correspondences than this and the observation is dropped. Any less and this is likely a degenerate solve.
50 */
52
53/*!
54 * Where a camera observation stops being treated as data and starts being treated as a mistake, in standard
55 * deviations. Used to size the Huber loss function.
56 *
57 * The loss is applied to a whole observation rather than to each LED of it, so this is measured against the spread of
58 * the observation's chi-square rather than against a single reprojection error, and the threshold it produces grows
59 * with the number of correspondences.
60 */
61constexpr double kCameraHuberDeltaSigmas = 3.0;
62
63using namespace xrt::auxiliary::math;
64using namespace xrt::auxiliary::tracking;
65
66using Eigen::Map;
67using Eigen::Matrix;
68using Eigen::Matrix3d;
69using Eigen::MatrixBase;
70using Eigen::MatrixXd;
71using Eigen::Quaternion;
72using Eigen::Quaterniond;
73using Eigen::Vector;
74using Eigen::Vector2;
75using Eigen::Vector3;
76using Eigen::Vector3d;
77using Eigen::Vector4;
78using Eigen::VectorXd;
79
80using ceres::Jet;
81
82
83/*
84 *
85 * Types
86 *
87 */
88
89template <typename T, int N> struct SeedableVector
90{
91 using VectorType = Vector<T, N>;
92
93public: // Fields
94 VectorType vec;
95 bool seeded{false};
96
97 /*!
98 * The value this state is held linearized around, once it has been anchored.
99 *
100 * A marginalization prior is a statement about one particular linearization of the states it covers, and it
101 * goes on making that statement long after the solve has moved them. Everything that touches an anchored
102 * state has to keep linearizing it here, or the frozen factor and the live residuals stop describing the
103 * same point.
104 */
105 VectorType first_estimate;
106
107 //! Whether @ref first_estimate has been taken.
108 bool anchored{false};
109
110public: // Methods
111 SeedableVector() = default;
112
113 /*!
114 * Returns the data to use for a parameter, asserts you have seeded the data before this.
115 */
116 T *
118 {
119 assert(this->seeded);
120 return this->vec.data();
121 }
122
123 /*!
124 * Returns the data to use for a parameter, asserts you have seeded the data before this.
125 */
126 const T *
127 data() const
128 {
129 assert(this->seeded);
130 return this->vec.data();
131 }
132
133 /*!
134 * Returns the vector for you to write into and seed. Assumes you have seeded it after this function is called.
135 */
136 VectorType &
138 {
139 this->seeded = true;
140 return this->vec;
141 }
142
143 //! Freezes the current value as the point this state is linearized at, unless it already has one.
144 void
146 {
147 assert(this->seeded);
148
149 if (this->anchored) {
150 return;
151 }
152
153 this->first_estimate = this->vec;
154 this->anchored = true;
155 }
156
157 //! The frozen linearization point, or null for a state that has never been anchored.
158 const T *
160 {
161 return this->anchored ? this->first_estimate.data() : nullptr;
162 }
163
164 //! Forgets the frozen linearization point, so the state linearizes wherever it currently is again.
165 void
167 {
168 this->anchored = false;
169 }
170
171 //! Drops everything held here, for a slot about to be handed to a state of its own.
172 void
174 {
175 this->seeded = false;
176 this->anchored = false;
177 }
178};
179
180/*
181 *
182 * Parameter block types
183 *
184 */
185
186/*!
187 * The strength of gravity in the world, as a parameter block of its own.
188 *
189 * This is a property of where the capture was taken rather than of any one sensor, so every IMU in a solve shares this
190 * single block while each keeps its own @ref ImuBias.
191 */
192template <typename T> struct WorldGravity
193{
194 static_assert(!std::is_const_v<T>,
195 "block scalar must not be const — did a `T*` overload deduce T = const double?");
196
197 static constexpr int kNumParameters = 1;
198
199 static constexpr int kGravityMagIndex = 0;
200
201 //! Gravity in meters per second squared.
203
205
207
208 WorldGravity(Eigen::Ref<const Vector<T, kNumParameters>> parameters) : gravity_mag(parameters[kGravityMagIndex])
209 {}
210
211 void
212 pack(Eigen::Ref<Vector<T, kNumParameters>> parameters) const
213 {
214 parameters[kGravityMagIndex] = this->gravity_mag;
215 }
216
217 //! Gravity as a vector, which points along +Y since world up is -Y in the OpenCV space the optimizer works in.
219 toVector() const
220 {
221 return Vector3<T>(T(0), this->gravity_mag, T(0));
222 }
223};
224
225//! A parameter block for a camera with 2 degrees of freedom, pitch and roll. Yaw is fixed to 0, and it has no position.
226template <typename T> struct PitchRoll
227{
228 static_assert(!std::is_const_v<T>,
229 "block scalar must not be const — did a `T*` overload deduce T = const double?");
230
231 static constexpr int kNumParameters = 2;
232
233 Vector2<T> pitch_roll;
234
235 void
236 toQuaternion(Quaternion<T> &q) const
237 {
238 // @todo don't optimize on euler angles, use something without singularities.
239 // what are the chances someone has a camera pointed 90 degrees down?
240
241 q = Quaternion<T>(Eigen::AngleAxis<T>(pitch_roll.y(), Vector3<T>::UnitZ()));
242 q *= Quaternion<T>(Eigen::AngleAxis<T>(pitch_roll.x(), Vector3<T>::UnitX()));
243 }
244
245 Eigen::Transform<T, 3, Eigen::Isometry>
246 toTransform() const
247 {
249 this->toQuaternion(q);
250
251 return Eigen::Transform<T, 3, Eigen::Isometry>(q);
252 }
253
254 PitchRoll(Eigen::Ref<const Vector<T, kNumParameters>> parameters) : pitch_roll(parameters) {}
255};
256
257/*!
258 * Pose with 6 degrees of freedom, translation and rotation.
259 *
260 * Stored as a translation and a unit quaternion, so the ambient block is one larger than the 6 degrees of freedom
261 * it stands for. @ref PoseManifold supplies the retraction that takes the tangent space back to 6.
262 */
263template <typename T> struct Pose
264{
265 static_assert(!std::is_const_v<T>,
266 "block scalar must not be const — did a `T*` overload deduce T = const double?");
267
268 //! Size of the parameter block handed to Ceres: (tX, tY, tZ), (qX, qY, qZ, qW).
269 static constexpr int kNumParameters = 7;
270 //! Degrees of freedom the manifold exposes to the solver.
271 static constexpr int kNumTangentParameters = 6;
272 static constexpr int kNumResiduals = 6;
273
274 Vector3<T> translation;
275 Quaternion<T> rotation;
276
277 Pose(Quaternion<T> quat) : translation(Eigen::Vector3<T>::Zero()), rotation(quat) {}
278
279 Pose(xrt_pose pose) : Pose(map_quat(pose.orientation).cast<T>().normalized())
280 {
281 this->translation = Vector3<T>(pose.position.x, pose.position.y, pose.position.z);
282 }
283
284 Pose(Eigen::Ref<const Vector<T, kNumParameters>> parameters)
285 : translation(parameters.template segment<3>(kPoseStatePosStart)),
286 rotation(Quaternion<T>(parameters.template segment<4>(kPoseStateRotStart)))
287 {}
288
289 template <typename TransformType> Pose(const Eigen::Transform<TransformType, 3, Eigen::Isometry> &transform)
290 {
291 this->translation = transform.translation().template cast<T>();
292 Quaternion<TransformType> q(transform.rotation());
293 this->rotation = q.template cast<T>();
294 }
295
296 void
297 pack(Eigen::Ref<Vector<T, kNumParameters>> parameters) const
298 {
299 parameters.template segment<3>(kPoseStatePosStart) = this->translation;
300 parameters.template segment<4>(kPoseStateRotStart) = this->rotation.coeffs();
301 }
302
303 Eigen::Transform<T, 3, Eigen::Isometry>
304 toTransform() const
305 {
306 return Eigen::Translation<T, 3>(translation.x(), translation.y(), translation.z()) *
307 Eigen::Transform<T, 3, Eigen::Isometry>(this->rotation);
308 }
309
310 void
311 getQuaternion(Quaternion<T> &q) const
312 {
313 q = this->rotation;
314 }
315
317 toXrtPose() const
318 {
319 xrt_pose pose;
320
321 map_vec3(pose.position) = this->translation.template cast<float>();
322 map_quat(pose.orientation) = this->rotation.coeffs().template cast<float>();
323
324 return pose;
325 }
326
327private:
328 Pose() = default;
329};
330
331/*!
332 * Pose with 9 degrees of freedom, translation, rotation, and linear velocity.
333 *
334 * Stored as a `Pose` with an extra 3-parameter vector for the velocity.
335 *
336 * See @ref PoseWithVelocityManifold for the correct way to optimize on this,
337 * which takes tangent space from 10 -> 9 parameters (quat exponential map).
338 */
339template <typename T> struct PoseWithVelocity : Pose<T>
340{
341 static_assert(!std::is_const_v<T>,
342 "block scalar must not be const — did a `T*` overload deduce T = const double?");
343
344 static constexpr int kNumParameters = Pose<T>::kNumParameters + 3;
345 static constexpr int kNumTangentParameters = Pose<T>::kNumTangentParameters + 3;
346
347 Vector3<T> velocity;
348
349 PoseWithVelocity(xrt_pose p) : Pose<T>(p), velocity(Vector3<T>::Zero()) {}
350
351 template <typename Derived>
352 PoseWithVelocity(xrt_pose p, const Vector3<Derived> &velocity)
353 : Pose<T>(p), velocity(velocity.template cast<T>())
354 {}
355
356 PoseWithVelocity(Eigen::Ref<const Vector<T, kNumParameters>> parameters)
357 : Pose<T>(parameters.template segment<Pose<T>::kNumParameters>(0)),
358 velocity(Vector3<T>(parameters.template segment<3>(Pose<T>::kNumParameters)))
359 {}
360
361 void
362 pack(Eigen::Ref<Vector<T, kNumParameters>> parameters) const
363 {
364 this->Pose<T>::pack(parameters.template segment<Pose<T>::kNumParameters>(0));
365 parameters.template segment<3>(Pose<T>::kNumParameters) = this->velocity;
366 }
367};
368
369/*!
370 * The LED model -> IMU rotation.
371 *
372 * Device keyframe states are LED model poses, which the camera factors constrain directly, and only the IMU factors
373 * see the IMU through this rotation on top of them. Solving for it then leaves the poses where the cameras put them,
374 * rather than having to walk them the opposite way to keep the LED model still.
375 */
376template <typename T> struct ImuExtrinsics
377{
378 static_assert(!std::is_const_v<T>,
379 "block scalar must not be const — did a `T*` overload deduce T = const double?");
380
381 //! (qX, qY, qZ, qW)
382 static constexpr int kNumParameters = 4;
383 //! Degrees of freedom
384 static constexpr int kNumTangentParameters = 3;
385
386 Quaternion<T> Q_model_imu;
387
388 template <typename Derived> ImuExtrinsics(Quaternion<Derived> quat) : Q_model_imu(quat.template cast<T>()) {}
389
390 ImuExtrinsics(Eigen::Ref<const Vector<T, kNumParameters>> parameters)
391 : Q_model_imu(Quaternion<T>(parameters.template segment<4>(0)))
392 {}
393
394 void
395 pack(Eigen::Ref<Vector<T, kNumParameters>> parameters) const
396 {
397 parameters.template segment<4>(0) = this->Q_model_imu.coeffs();
398 }
399
400 /*!
401 * The IMU's state given the LED model's. The two share an origin, so only the orientation differs, and the
402 * velocity, being the origin's, carries straight over.
403 */
404 PoseWithVelocity<T>
405 toWorldImu(const PoseWithVelocity<T> &T_world_model) const
406 {
407 PoseWithVelocity<T> T_world_imu = T_world_model;
408 T_world_imu.rotation = T_world_model.rotation * this->Q_model_imu;
409
410 return T_world_imu;
411 }
412};
413
414/*
415 *
416 * Manifolds
417 *
418 */
419
420/*!
421 * Right-multiplying quaternion manifold, matches our local convention.
422 */
424{
425 template <typename T>
426 bool
427 Plus(const T *x, const T *delta, T *x_plus_delta) const
428 {
429 const Eigen::Map<const Eigen::Quaternion<T>> q(x);
430 const Eigen::Map<const Eigen::Vector3<T>> w(delta);
431 Eigen::Map<Eigen::Quaternion<T>> result(x_plus_delta);
432
433 result = q * quat_exp_so3(w);
434 return true;
435 }
436
437 template <typename T>
438 bool
439 Minus(const T *y, const T *x, T *y_minus_x) const
440 {
441 const Eigen::Map<const Eigen::Quaternion<T>> q_y(y);
442 const Eigen::Map<const Eigen::Quaternion<T>> q_x(x);
443 Eigen::Map<Eigen::Vector3<T>> result(y_minus_x);
444
445 result = quat_ln_so3(Eigen::Quaternion<T>(q_x.conjugate() * q_y));
446 return true;
447 }
448};
449
450//! Our quaternion manifold functor
451typedef ceres::AutoDiffManifold<QuaternionManifoldFunctor, 4, 3> QuaternionManifold;
452
453//! Pose manifold, (tX, tY, tZ), (qX, qY, qZ, qW)
454typedef ceres::ProductManifold<ceres::EuclideanManifold<3>, QuaternionManifold> PoseManifold;
455
456//! Pose plus linear velocity, (tX, tY, tZ), (qX, qY, qZ, qW), (vX, vY, vZ). 10 ambient, 9 tangent.
457typedef ceres::ProductManifold<ceres::EuclideanManifold<3>, //
459 ceres::EuclideanManifold<3>> //
461
462
463/*
464 *
465 * Helper functions
466 *
467 */
468
469//! Converts an @ref xrt_pose into an Eigen Isometry3f
470inline Eigen::Isometry3f
472{
473 return {Eigen::Translation3f{map_vec3(p.position)} * map_quat(p.orientation)};
474}
475
476/*!
477 * Finds a LED in the model by its id.
478 *
479 * A LED's id is an opaque identifier chosen by the driver, not its position in the model: drivers are free to
480 * derive it from device-specific numbering (the Rift uses the headset's own LED indices, which include a slot for
481 * the IMU). So a blob's matched_device_led_id has to be resolved through here rather than used as an index.
482 *
483 * @return The index of the LED in the model, or -1 when no LED carries that id.
484 */
485static inline int32_t
486findLedIndexById(const t_constellation_tracker_led_model *leds_model, t_constellation_led_id_it led_id)
487{
488 for (size_t i = 0; i < leds_model->led_count; i++) {
489 if (leds_model->leds[i].id == led_id) {
490 return static_cast<int32_t>(i);
491 }
492 }
493
494 return -1;
495}
496
497/*!
498 * Measures how well one kind of factor was fit, at whatever the problem's parameters currently hold.
499 *
500 * Both factor types are whitened, so this is directly comparable between them and against 1.
501 *
502 * @param problem The problem, evaluated at its current parameter values.
503 * @param residual_blocks The blocks belonging to this kind of factor.
504 */
506evaluateFactorFitness(ceres::Problem &problem, const std::vector<ceres::ResidualBlockId> &residual_blocks);
507
508//! Converts a CRS matrix to a dense Eigen matrix
509void
510matrixCrsToEigen(const ceres::CRSMatrix &mat_crs, Eigen::MatrixXd &mat);
511
512/*!
513 * Marginalize the first @p num_base_states states out of the linearized system given by @p jacobian and
514 * @p residual, handing back the linear prior that replaces them.
515 *
516 * Inspired/specialized from MarginalizationError::marginalizeOut() from OKVIS.
517 */
518void
519marginalize(const VectorXd &residual, //
520 const MatrixXd &jacobian, //
521 const int num_base_states, //
522 VectorXd &out_e0, //
523 MatrixXd &out_j); //
524
525/*
526 *
527 * LED residuals
528 *
529 */
530
531//! X and Y in the image, for one LED.
532constexpr int kNumLedResiduals = 2;
533
534/*!
535 * How well a blob centroid locates the LED that lit it, in pixels.
536 *
537 * The camera residuals are reprojection errors, so they are only comparable to the whitened IMU residuals once they
538 * are divided by the noise that produced them. This is the same floor the per-frame PnP puts on its own residual
539 * sigma, since a blob has about this much pixels of noise, at least on CV1.
540 */
541constexpr double kBlobPositionSigmaPixels = 0.16;
542
543template <typename T>
544static void
545project_led(const camera_model &params,
546 const Eigen::Quaternion<T> &Q_cam_model,
547 const Eigen::Vector3<T> &T_cam_model,
548 const Eigen::Vector3<T> &T_model_led,
549 Eigen::Vector2<T> &out_projected_point,
550 T *led_depth_m)
551{
552 // Rotate the point around the pose's local frame.
553 Eigen::Vector3<T> T_cam_led = Q_cam_model * T_model_led;
554 // Translate the point into the camera frame.
555 T_cam_led += T_cam_model;
556
557 if (led_depth_m != nullptr) {
558 *led_depth_m = T_cam_led.z();
559 }
560
561 Eigen::Vector2<T> projected_point = {T(1e6), T(1e6)};
562
563 if (T_cam_led.z() <= T(0)) {
564 // The point is behind the camera, so we can't project it. Return a large value to indicate this.
565 out_projected_point = projected_point;
566 return;
567 }
568
570
571 (void)camera_models::project(dist, //
572 T_cam_led.x(), //
573 T_cam_led.y(), //
574 T_cam_led.z(), //
575 projected_point.x(), //
576 projected_point.y()); //
577
578 out_projected_point = projected_point;
579}
580
581template <typename T, typename Derived>
582static void
583computeLedResidual(const camera_model &params,
584 const Eigen::Vector3<T> &T_cam_model,
585 const Eigen::Quaternion<T> &Q_cam_model,
586 const Eigen::Vector2<T> &blob_position_2d,
587 const Eigen::Vector3<T> &T_model_led,
588 Eigen::MatrixBase<Derived> &residual)
589{
590 Eigen::Vector2<T> predicted_point;
591 T led_depth;
592 project_led<T>(params, //
593 Q_cam_model, //
594 T_cam_model, //
595 T_model_led, //
596 predicted_point, //
597 &led_depth); //
598
599 if constexpr (kResidualDebugPrint) {
600 if constexpr (std::is_same_v<T, double>) {
601 U_LOG_W("meas=(%f,%f)", blob_position_2d.x(), blob_position_2d.y());
602 U_LOG_W("pred=(%f,%f)", predicted_point.x(), predicted_point.y());
603
604 U_LOG_W("pose=(%f,%f,%f), (%f,%f,%f)", T_cam_model.x(), T_cam_model.y(), T_cam_model.z(),
605 Q_cam_model.x(), Q_cam_model.y(), Q_cam_model.z());
606
607 if (std::isnan(predicted_point.x()) || std::isnan(predicted_point.y())) {
608 U_LOG_W("Projected point is NaN, led_depth=%f", led_depth);
609
610 // XRT_DEBUGBREAK();
611 }
612 } else {
613 U_LOG_W("meas=(%f,%f)", blob_position_2d.x().a, blob_position_2d.y().a);
614 U_LOG_W("pred=(%f,%f)", predicted_point.x().a, predicted_point.y().a);
615
616 U_LOG_W("pose=(%f,%f,%f), (%f,%f,%f)", T_cam_model.x().a, T_cam_model.y().a, T_cam_model.z().a,
617 Q_cam_model.x().a, Q_cam_model.y().a, Q_cam_model.z().a);
618
619 if (std::isnan(predicted_point.x().a) || std::isnan(predicted_point.y().a)) {
620 U_LOG_W("Projected point is NaN, led_depth=%f", led_depth.a);
621
622 // XRT_DEBUGBREAK();
623 }
624 }
625 }
626
627 // Compute the residual
628 residual = predicted_point - blob_position_2d;
629
630 /*
631 * Provide a smooth penalty for points behind the camera, since if we just set a large residual,
632 * the solver will have no derivative to try to work back from. This solution penalizes bad
633 * poses, while pushing the optimizer towards the correct solution (no LEDs behind the camera).
634 */
635 if (led_depth < T(0.001)) {
636 residual += Eigen::Vector2<T>::Constant(T(1000) * (T(0.001) - T(led_depth)));
637 }
638}
639
640}; // namespace xrt::tracking::constellation::optimizer
Definition utility_northstar.h:779
Definition utility_northstar.h:270
Definition utility_northstar.h:16
#define U_LOG_W(...)
Log a message at U_LOGGING_WARN level, conditional on the global log level.
Definition u_logging.h:439
#define MATH_GRAVITY_M_S2
Standard gravity acceleration constant.
Definition m_api.h:53
void marginalize(const VectorXd &residual, const MatrixXd &jacobian, const int num_base_states, VectorXd &out_e0, MatrixXd &out_j)
Marginalize the first num_base_states states out of the linearized system given by jacobian and resid...
Definition internal_math.cpp:345
void matrixCrsToEigen(const ceres::CRSMatrix &mat_crs, Eigen::MatrixXd &mat)
Converts a CRS matrix to a dense Eigen matrix.
Definition internal_math.cpp:325
FactorFitness evaluateFactorFitness(ceres::Problem &problem, const std::vector< ceres::ResidualBlockId > &residual_blocks)
Measures how well one kind of factor was fit, at whatever the problem's parameters currently hold.
Definition internal_math.cpp:274
constexpr size_t kMinCorrespondencesPerObservation
Fewer correspondences than this and the observation is dropped.
Definition internal_math.hpp:51
constexpr double kCameraHuberDeltaSigmas
Where a camera observation stops being treated as data and starts being treated as a mistake,...
Definition internal_math.hpp:61
ceres::ProductManifold< ceres::EuclideanManifold< 3 >, QuaternionManifold, ceres::EuclideanManifold< 3 > > PoseWithVelocityManifold
Pose plus linear velocity, (tX, tY, tZ), (qX, qY, qZ, qW), (vX, vY, vZ). 10 ambient,...
Definition internal_math.hpp:460
static int32_t findLedIndexById(const t_constellation_tracker_led_model *leds_model, t_constellation_led_id_it led_id)
Finds a LED in the model by its id.
Definition internal_math.hpp:486
constexpr bool kResidualDebugPrint
Print debug info about the residuals as we're computing them.
Definition internal_math.hpp:43
Eigen::Isometry3f isometryFromPose(const xrt_pose &p)
Converts an xrt_pose into an Eigen Isometry3f.
Definition internal_math.hpp:471
ceres::ProductManifold< ceres::EuclideanManifold< 3 >, QuaternionManifold > PoseManifold
Pose manifold, (tX, tY, tZ), (qX, qY, qZ, qW)
Definition internal_math.hpp:454
constexpr int kNumLedResiduals
X and Y in the image, for one LED.
Definition internal_math.hpp:532
constexpr double kBlobPositionSigmaPixels
How well a blob centroid locates the LED that lit it, in pixels.
Definition internal_math.hpp:541
ceres::AutoDiffManifold< QuaternionManifoldFunctor, 4, 3 > QuaternionManifold
Our quaternion manifold functor.
Definition internal_math.hpp:451
constexpr bool kOptimizeUndistortedPoints
Optimize directly on the pre-undistorted points, don't compute residuals in distorted pixel space.
Definition internal_math.hpp:41
Interoperability helpers connecting internal math types and Eigen.
Base implementations for math library.
Helpers for quatexpmap math for bigceres usage.
Generic math helpers for constellation optimizer code.
C++-only functionality in the Math helper library.
Definition m_documentation.hpp:15
Eigen::Quaternion< typename Derived::Scalar > quat_exp_so3(Eigen::MatrixBase< Derived > const &vec)
Fully-templated free function for quaternion exponentiation, SO(3) version as described by Grassia.
Definition m_quatexpmap.hpp:119
static Eigen::Map< const Eigen::Quaternionf > map_quat(const struct xrt_quat &q)
Wrap an internal quaternion struct in an Eigen type, const overload.
Definition m_eigen_interop.hpp:40
static Eigen::Map< const Eigen::Quaternionf > orientation(const struct xrt_pose &pose)
Return a Eigen type wrapping a pose's orientation (const).
Definition m_eigen_interop.hpp:232
Eigen::Matrix< Scalar, 3, 1 > quat_ln_so3(Eigen::Quaternion< Scalar > const &quat)
Fully-templated free function for quaternion log map, SO(3) version.
Definition m_quatexpmap.hpp:148
static Eigen::Map< const Eigen::Vector3f > map_vec3(const struct xrt_vec3 &v)
Wrap an internal 3D vector struct in an Eigen type, const overload.
Definition m_eigen_interop.hpp:90
RANSAC PnP pose refinement.
Definition camera_model.h:21
struct t_camera_model_params calib_pinhole
An identity calibration model.
Definition camera_model.h:38
struct t_camera_model_params calib_true
The true calibration of the camera.
Definition camera_model.h:36
Floating point calibration data for a single calibrated camera.
Definition t_camera_models.h:62
The LED model is a series of points which define the real-world positions of all LEDs.
Definition t_constellation.h:313
uint32_t led_count
The number of LEDs in this model.
Definition t_constellation.h:317
struct t_constellation_tracker_led * leds
The LEDs in this model.
Definition t_constellation.h:315
t_constellation_led_id_it id
A unique ID for this LED, which distinguishes it from all other LEDs.
Definition t_constellation.h:290
How well one kind of sensor was fit, so that a bad solve can be attributed to the sensor that could n...
Definition math.hpp:31
The LED model -> IMU rotation.
Definition internal_math.hpp:377
static constexpr int kNumTangentParameters
Degrees of freedom.
Definition internal_math.hpp:384
static constexpr int kNumParameters
(qX, qY, qZ, qW)
Definition internal_math.hpp:382
PoseWithVelocity< T > toWorldImu(const PoseWithVelocity< T > &T_world_model) const
The IMU's state given the LED model's.
Definition internal_math.hpp:405
A parameter block for a camera with 2 degrees of freedom, pitch and roll. Yaw is fixed to 0,...
Definition internal_math.hpp:227
Pose with 6 degrees of freedom, translation and rotation.
Definition internal_math.hpp:264
static constexpr int kNumParameters
Size of the parameter block handed to Ceres: (tX, tY, tZ), (qX, qY, qZ, qW).
Definition internal_math.hpp:269
static constexpr int kNumTangentParameters
Degrees of freedom the manifold exposes to the solver.
Definition internal_math.hpp:271
Pose with 9 degrees of freedom, translation, rotation, and linear velocity.
Definition internal_math.hpp:340
Right-multiplying quaternion manifold, matches our local convention.
Definition internal_math.hpp:424
void unseed()
Drops everything held here, for a slot about to be handed to a state of its own.
Definition internal_math.hpp:173
const T * data() const
Returns the data to use for a parameter, asserts you have seeded the data before this.
Definition internal_math.hpp:127
bool anchored
Whether first_estimate has been taken.
Definition internal_math.hpp:108
const T * anchorData() const
The frozen linearization point, or null for a state that has never been anchored.
Definition internal_math.hpp:159
T * data()
Returns the data to use for a parameter, asserts you have seeded the data before this.
Definition internal_math.hpp:117
VectorType first_estimate
The value this state is held linearized around, once it has been anchored.
Definition internal_math.hpp:105
VectorType & seedVec()
Returns the vector for you to write into and seed.
Definition internal_math.hpp:137
void releaseAnchor()
Forgets the frozen linearization point, so the state linearizes wherever it currently is again.
Definition internal_math.hpp:166
void anchor()
Freezes the current value as the point this state is linearized at, unless it already has one.
Definition internal_math.hpp:145
The strength of gravity in the world, as a parameter block of its own.
Definition internal_math.hpp:193
T gravity_mag
Gravity in meters per second squared.
Definition internal_math.hpp:202
Vector3< T > toVector() const
Gravity as a vector, which points along +Y since world up is -Y in the OpenCV space the optimizer wor...
Definition internal_math.hpp:219
A pose composed of a position and orientation.
Definition xrt_defines.h:522
Camera (un)projection C++ API for various camera models.